#include "Mouse_Drive.h"
/*********************************************************************************************************
  定义全局变量
*********************************************************************************************************/
MAZECOOR GmcMouse = {0, 0};                   /*  保存电脑鼠当前位置坐标      */
uint8 GucMouseDir = UP;                       /*  保存电脑鼠当前方向          */
uint8 GucMapBlock[MAZETYPE][MAZETYPE] = {0};  /*  GucMapBlock[x][y]           */
                                              /*  x,横坐标;y,纵坐标;          */
uint8 GucMapBlock0[MAZETYPE][MAZETYPE] = {0}; /*  中心算法         */
                                              /*  bit3~bit0分别代表左下右上   */
                                              /*  0:该方向无路，1:该方向有路  */
uint8 GucMapBlock1[MAZETYPE][MAZETYPE] = {0x0f};
uint8 GucMouseStart = 0; /* 电脑鼠启动        */
uint8 GucFrontJinju = 0; /* 前方红外近距在搜索时等高图制作中用   */
uint8 GucCrossroad = 0;  /* 十字路口数，冲刺时用，若十字路口多降低最高冲刺速度   */
static uint32 GW;        /*小车转动角度*/

static __MOTOR __GmLeft = {0, 0, 0, 0, 0, 0};  /*  定义并初始化左电机状态      */
static __MOTOR __GmRight = {0, 0, 0, 0, 0, 0}; /*  定义并初始化右电机状态      */
static __PID __GmLPID;                         /*  定义左电机PID      */
static __PID __GmRPID;                         /*  定义右电机PID     */
static __PID __GmSPID;                         /*  直线PID     */
static __PID __GmWPID;                         /*  旋转PID     */
static uint8 __GucMouseState = __STOP;         /*  保存电脑鼠当前运行状态      */
static int32 __GiMaxSpeed = SEARCHSPEED;       /*  保存允许运行的最大速度      */
static int32 __GiMinSpeed = SEARCHSPEED;
static uint8 __GucDistance[5] = {0}; /*  记录传感器状态              */
uint16 GusFreq_F = 36200;            //33.8,33,327        /*  前方红外频率              */
uint16 GusFreq_FJ = 19200;           //26.3,266,275              /*  前方近距红外频率              */
uint16 GusFreq_X = 30000;            //35,33.8          /*  斜45度红外频率              */
uint16 GusFreq_LF = 31700;           //34000           /*  左右红外远距频率              */
uint16 GusFreq_L = 18300;            /*  左右红外近距频率              */
static int16 GsTpusle_T = 0;         /*  左轮校正减少的速度值              */
static uint8 GuiSpeedCtr = 0;
static uint16 GuiTpusle_LR = 0;
static uint16 GuiTpusle_S = 0;
static uint16 GuiTpusle_K = 0;
static uint8 GucFrontNear = 0;
uint8 GucGoHead = 0;
uint8 GucGoHead1 = 0;
uint8 GucGoHead2 = 0;
uint8 GucFangXiang = 0;
uint8 GucDirTemp = 0;
uint16 DIS[10] = {0};
uint8 Tab = 4; //红外调试选项
uint16 count = 0;
extern u16 voltageDetectRef;
extern __IO uint16_t ADC_ConvertedValue[5];
extern float ADC_ConvertedValueLocal[4];
float W;
uint16 Sunlight[4];

/*
 * ucIRCheck[2] 红外左
 * ucIRCheck[3] 红外右
 */
uint16 ucIRCheck[4];
//红外的设定值，最后改成数组形式
uint16 GusDistance_L_Near = 461;  //左红外近
uint16 GusDistance_L_Mid = 384;   //左红外中
uint16 GusDistance_L_Far = 110;   //左红外远
uint16 GusDistance_R_Near = 409;  //右红外近
uint16 GusDistance_R_Mid = 329;   //右红外中
uint16 GusDistance_R_Far = 100;   //右红外近
uint16 GusDistance_FL_Near = 200; //近距，用来判断停止
uint16 GusDistance_FR_Near = 200;
uint16 GusDistance_FL_Far = 110; //远距用来判断墙壁
uint16 GusDistance_FR_Far = 200;
u8 flag = 0;
u8 ir_flag = 0;
u8 mm = 0;
u8 backflag = 0;
u8 backflag2 = 0;
u16 time;
/*********************************************************************************************************
** Function name:       __delay
** Descriptions:        延时函数
** input parameters:    uiD :延时参数，值越大，延时越久
** output parameters:   无
** Returned value:      无
*********************************************************************************************************/
void __delay(uint32 uiD)
{
  for (; uiD; uiD--)
    ;
}
void Delay_us(u32 Nus)
{
  SysTick->LOAD = Nus * 9;
  SysTick->CTRL |= 0x01;
  while (!(SysTick->CTRL & (1 << 16)))
    ;
  SysTick->CTRL = 0X00000000;
  SysTick->VAL = 0X00000000;
}
/*********************************************************************************************************
** Function name:       PIDInit
** Descriptions:        PID初始化
** input parameters:    无
** output parameters:   无
** Returned value:      无
*********************************************************************************************************/
void PIDInit(void)
{
  __GmLPID.usEncoder_new = 32768;
  //__GmLPID.usEncoder_last = 65535;
  __GmLPID.usFeedBack = 0; //速度反馈值
  __GmLPID.sFeedBack = 0;

  __GmRPID.usEncoder_new = 32768;
  //__GmRPID.usEncoder_last = 65535;
  __GmRPID.usFeedBack = 0; //速度反馈值
  __GmRPID.sFeedBack = 0;

  __GmSPID.sRef = 0; //速度设定值
  __GmSPID.sFeedBack = 0;
  __GmSPID.sPreError = 0;  //前一次，速度误差,,vi_Ref - vi_FeedBack
  __GmSPID.sPreDerror = 0; //前一次，速度误差之差，d_error-PreDerror;

  __GmSPID.fKp = __KP;
  __GmSPID.fKi = __KI;
  __GmSPID.fKd = __KD;

  __GmSPID.iPreU = 0; //电机控制输出值

  __GmWPID.sRef = 0; //速度设定值
  __GmWPID.sFeedBack = 0;
  __GmWPID.sPreError = 0;  //前一次，速度误差,,vi_Ref - vi_FeedBack
  __GmWPID.sPreDerror = 0; //前一次，速度误差之差，d_error-PreDerror;

  __GmWPID.fKp = __KP; //30
  __GmWPID.fKi = __KI; //0.1,0.01
  __GmWPID.fKd = __KD;

  __GmWPID.iPreU = 0; //电机控制输出值
}

/*********************************************************************************************************
** Function name:       __SPIDContr
** Descriptions:        直线PID控制
** input parameters:    无
** output parameters:   无
** Returned value:      无
*********************************************************************************************************/
void __SPIDContr(void)
{
  float error, d_error, dd_error;
  static uint8 K_I = 1;
  error = __GmSPID.sRef - __GmSPID.sFeedBack; // 偏差计算
  d_error = error - __GmSPID.sPreError;
  dd_error = d_error - __GmSPID.sPreDerror;
  if (error > Deadband)
    error -= Deadband;
  else if (error < -Deadband)
    error += Deadband;
  else
    error = 0;
  if ((error > error_IMAX) || (error < -error_IMAX))
    K_I = 0;
  else
    K_I = 1;

  __GmSPID.sPreError = error; //存储当前偏差
  __GmSPID.sPreDerror = d_error;

  __GmSPID.iPreU += (int16)(__GmSPID.fKp * d_error + K_I * __GmSPID.fKi * error + __GmSPID.fKd * dd_error);
}
/*********************************************************************************************************
** Function name:       __WPIDContr
** Descriptions:        旋转方向PID控制
** input parameters:    无
** output parameters:   无
** Returned value:      无
*********************************************************************************************************/
void __WPIDContr(void)
{
  float error, d_error, dd_error;
  static uint8 K_I = 1;
  error = __GmWPID.sRef + GsTpusle_T - __GmWPID.sFeedBack; // 偏差计算
  d_error = error - __GmWPID.sPreError;
  dd_error = d_error - __GmWPID.sPreDerror;
  if (error > Deadband)
    error -= Deadband;
  else if (error < -Deadband)
    error += Deadband;
  else
    error = 0;
  if ((error > error_IMAX) || (error < -error_IMAX))
    K_I = 0;
  else
    K_I = 1;

  __GmWPID.sPreError = error; //存储当前偏差
  __GmWPID.sPreDerror = d_error;
  __GmWPID.iPreU += (int16)(__GmWPID.fKp * d_error + K_I * __GmWPID.fKi * error + __GmWPID.fKd * dd_error);
}

/*********************************************************************************************************
** Function name:      __PIDContr
** Descriptions:        PID控制，通过脉冲数控制电机
** input parameters:    无
** output parameters:   无
** Returned value:      无
*********************************************************************************************************/
void __PIDContr(void)
{
  __SPIDContr();
  __WPIDContr();
  __GmLeft.sSpeed = __GmSPID.iPreU - __GmWPID.iPreU;
  if (__GmLeft.sSpeed >= 0)
  {
    __GmLeft.cDir = __MOTORGOAHEAD;
    if (__GmLeft.sSpeed >= U_MAX) //速度PID，防止调节最高溢出
      __GmLeft.sSpeed = U_MAX;
    if (__GmLeft.sSpeed <= U_MIN) //速度PID，防止调节最低溢出
      __GmLeft.sSpeed = U_MIN;
  }
  else
  {
    __GmLeft.cDir = __MOTORGOBACK;
    __GmLeft.sSpeed *= -1;
    if (__GmLeft.sSpeed >= U_MAX) //速度PID，防止调节最高溢出
      __GmLeft.sSpeed = U_MAX;
    if (__GmLeft.sSpeed <= U_MIN) //速度PID，防止调节最低溢出
      __GmLeft.sSpeed = U_MIN;
  }

  __GmRight.sSpeed = __GmSPID.iPreU + __GmWPID.iPreU;
  if (__GmRight.sSpeed >= 0)
  {
    __GmRight.cDir = __MOTORGOAHEAD;
    if (__GmRight.sSpeed >= U_MAX) //速度PID，防止调节最高溢出
      __GmRight.sSpeed = U_MAX;
    if (__GmRight.sSpeed <= U_MIN) //速度PID，防止调节最低溢出
      __GmRight.sSpeed = U_MIN;
  }
  else
  {
    __GmRight.cDir = __MOTORGOBACK;
    __GmRight.sSpeed *= -1;
    if (__GmRight.sSpeed >= U_MAX) //速度PID，防止调节最高溢出
      __GmRight.sSpeed = U_MAX;
    if (__GmRight.sSpeed <= U_MIN) //速度PID，防止调节最低溢出
      __GmRight.sSpeed = U_MIN;
  }
  __rightMotorContr();
  __leftMotorContr();
}
/*********************************************************************************************************
** Function name:       __Encoder
** Descriptions:        采集编码器输出的脉冲
** input parameters:    无
** output parameters:   无
** Returned value:      无
*********************************************************************************************************/
void __Encoder(void)
{
  static u16 Dir_L;
  static u16 Dir_R;

  __GmLPID.usEncoder_new = TIM_GetCounter(TIM2);
  __GmRPID.usEncoder_new = TIM_GetCounter(TIM3);

  Dir_R = TIM3->CR1;
  Dir_R = (Dir_R & 0x0010) >> 4;

  Dir_L = TIM2->CR1;
  Dir_L = (Dir_L & 0x0010) >> 4;

  if (Dir_L == 1) //向下计数  向后退
  {
    __GmLPID.usFeedBack = 32768 - __GmLPID.usEncoder_new;
    if (__GmLPID.usFeedBack > 400)
      __GmLPID.usFeedBack = 350;
    //__GmLPID.usEncoder_last = __GmLPID.usEncoder_new;
    TIM_SetCounter(TIM2, 32768);
    __GmLeft.uiPulseCtr += __GmLPID.usFeedBack;
    __GmLeft.cRealDir = __MOTORGOBACK;
    __GmLPID.sFeedBack = -1 * __GmLPID.usFeedBack;
  }
  else
  {
    __GmLPID.usFeedBack = __GmLPID.usEncoder_new - 32768;
    TIM_SetCounter(TIM2, 32768);
    if (__GmLPID.usFeedBack > 400)
      __GmLPID.usFeedBack = 350;
    __GmLeft.uiPulseCtr += __GmLPID.usFeedBack;
    __GmLeft.cRealDir = __MOTORGOAHEAD;
    __GmLPID.sFeedBack = __GmLPID.usFeedBack;
  }

  if (Dir_R == 1) //向下计数  前进
  {
    __GmRPID.usFeedBack = 32768 - __GmRPID.usEncoder_new;
    if (__GmRPID.usFeedBack > 400)
      __GmRPID.usFeedBack = 350;
    TIM_SetCounter(TIM3, 32768);
    __GmRight.uiPulseCtr += __GmRPID.usFeedBack;
    __GmRight.cRealDir = __MOTORGOAHEAD;
    __GmRPID.sFeedBack = __GmRPID.usFeedBack;
  }
  else
  {
    __GmRPID.usFeedBack = __GmRPID.usEncoder_new - 32768;
    TIM_SetCounter(TIM3, 32768);
    if (__GmRPID.usFeedBack > 400)
      __GmRPID.usFeedBack = 350;
    __GmRight.uiPulseCtr += __GmRPID.usFeedBack;
    __GmRight.cRealDir = __MOTORGOBACK;
    __GmRPID.sFeedBack = -1 * __GmRPID.usFeedBack;
  }

  __GmSPID.sFeedBack = (__GmRPID.sFeedBack + __GmLPID.sFeedBack) / 2;
  __GmWPID.sFeedBack = (__GmRPID.sFeedBack - __GmLPID.sFeedBack) / 2;
}
void __irSendFreq(int8 __cNumber)
{
  switch (__cNumber)
  {
  case 1:                            /*前方红外*/
    GPIO_SetBits(GPIOA, GPIO_Pin_5); //前方左 开
    break;

  case 2:                             /*左右红外*/
    GPIO_SetBits(GPIOC, GPIO_Pin_13); //前方右 开
    break;
  case 3:
    GPIO_SetBits(GPIOA, GPIO_Pin_3); //左 开
    break;
  case 4:
    GPIO_SetBits(GPIOC, GPIO_Pin_2); //右 开
    break;
  default:
    break;
  }
}

void __irCheck(void)
{
  static uint8 ucState = 0;
  switch (ucState)
  {
  case 0:
    ucIRCheck[3] = ADC_ConvertedValue[3] - Sunlight[3];
    if ((ucIRCheck[3] < 0) || (ucIRCheck[3] > 35000))
      ucIRCheck[3] = 0;
    GPIO_ResetBits(GPIOC, GPIO_Pin_2);
    if (ucIRCheck[3] > GusDistance_R_Far)
    {
      __GucDistance[__RIGHT] |= 0x01;
    }
    else
    {
      __GucDistance[__RIGHT] &= 0xfe;
    }

    if (ucIRCheck[3] > GusDistance_R_Mid)
    {
      __GucDistance[__RIGHT] |= 0x02;
    }

    else
    {
      __GucDistance[__RIGHT] &= 0xfd;
    }

    if (ucIRCheck[3] > GusDistance_R_Near)
    {
      __GucDistance[__RIGHT] |= 0x04;
    }

    else
    {
      __GucDistance[__RIGHT] &= 0xfb;
    }
    Sunlight[0] = 0;
    delay(1000);
    Sunlight[0] = ADC_ConvertedValue[0];
    __irSendFreq(1);
    break;

  case 1:
    ucIRCheck[0] = ADC_ConvertedValue[0] - Sunlight[0];
    if ((ucIRCheck[0] < 0) || (ucIRCheck[0] > 30000))
      ucIRCheck[0] = 0;
    GPIO_ResetBits(GPIOA, GPIO_Pin_5);

    if (ucIRCheck[0] > GusDistance_FL_Far)
    {
      __GucDistance[__FRONTL] |= 0x01;
    }
    else
    {
      __GucDistance[__FRONTL] &= 0xfe;
    }
    if (ucIRCheck[0] > GusDistance_FL_Near)
    {
      __GucDistance[__FRONTL] |= 0x02;
    }
    else
    {
      __GucDistance[__FRONTL] &= 0xfd;
    }
    Sunlight[1] = 0;
    delay(1000);
    Sunlight[1] = ADC_ConvertedValue[1];
    // __irSendFreq(2); //前方右 开
    GPIO_SetBits(GPIOC, GPIO_Pin_13); // 打开前方右红外线
    break;

  case 2:
    ucIRCheck[1] = ADC_ConvertedValue[1] - Sunlight[1];
    if ((ucIRCheck[1] < 0) || (ucIRCheck[1] > 30000))
      ucIRCheck[1] = 0;
    GPIO_ResetBits(GPIOC, GPIO_Pin_13); // 关闭前右红外线
    if (ucIRCheck[1] > GusDistance_FR_Far)
    {
      __GucDistance[__FRONTR] |= 0x01;
    }
    else
    {
      __GucDistance[__FRONTR] &= 0xfe;
    }

    if (ucIRCheck[1] > GusDistance_FL_Near)
    {
      __GucDistance[__FRONTR] |= 0x02;
    }
    else
    {
      __GucDistance[__FRONTR] &= 0xfd;
    }
    if ((ucIRCheck[0] > GusDistance_FL_Far) && (ucIRCheck[1] > GusDistance_FR_Far))
    {
      GucGoHead = 1;
    }

    else
    {
      GucGoHead = 0;
    }

    Sunlight[2] = 0;
    delay(1000);
    Sunlight[2] = ADC_ConvertedValue[2];
    __irSendFreq(3);
    break;

  case 3:
    ucIRCheck[2] = ADC_ConvertedValue[2] - Sunlight[2];
    if ((ucIRCheck[2] < 0) || (ucIRCheck[2] > 30000))
      ucIRCheck[2] = 0;
    GPIO_ResetBits(GPIOA, GPIO_Pin_3);
    if (ucIRCheck[2] > GusDistance_L_Far)
    {
      __GucDistance[__LEFT] |= 0x01;
    }
    else
    {
      __GucDistance[__LEFT] &= 0xfe;
    }

    if (ucIRCheck[2] > GusDistance_L_Mid)
    {
      __GucDistance[__LEFT] |= 0x02;
    }
    else
    {
      __GucDistance[__LEFT] &= 0xfd;
    }

    if (ucIRCheck[2] > GusDistance_L_Near)
    {
      __GucDistance[__LEFT] |= 0x04;
    }
    else
    {
      __GucDistance[__LEFT] &= 0xfb;
    }
    Sunlight[3] = 0;
    delay(1000);
    Sunlight[3] = ADC_ConvertedValue[3];
    __irSendFreq(4);
    break;
  default:
    break;
  }
  ucState = (ucState + 1) % 4;
}

void sensorDebug(void)
{
  zlg7289Download(2, 0, 0, __GucDistance[__LEFT]);   //传感器左
  zlg7289Download(2, 1, 0, __GucDistance[__FRONTL]); //传感器前左
  zlg7289Download(2, 2, 0, __GucDistance[__FRONTR]); //传感器前右
  zlg7289Download(2, 3, 0, __GucDistance[__RIGHT]);  //传感器右
}

/*********************************************************************************************************
** Function name:       __rightMotorContr
** Descriptions:        右直流电机驱动
** input parameters:    无
** output parameters:   无
** Returned value:      无
*********************************************************************************************************/
void __rightMotorContr(void)
{
  switch (__GmRight.cDir)
  {
  case __MOTORGOAHEAD:
    TIM_SetCompare1(TIM1, __GmRight.sSpeed);
    TIM_SetCompare2(TIM1, 0);
    break;

  case __MOTORGOBACK:
    TIM_SetCompare1(TIM1, 0);
    TIM_SetCompare2(TIM1, __GmRight.sSpeed);
    break;

  case __MOTORGOSTOP:
    TIM_SetCompare1(TIM1, 0);
    TIM_SetCompare2(TIM1, 0);
    break;

  default:
    break;
  }
}

/*********************************************************************************************************
** Function name:       __leftMotorContr
** Descriptions:        左直流电机驱动
** input parameters:    __GmLeft.cDir :电机运行方向
** output parameters:   无
** Returned value:      无
*********************************************************************************************************/
void __leftMotorContr(void)
{
  switch (__GmLeft.cDir)
  {
  case __MOTORGOAHEAD:
    TIM_SetCompare4(TIM1, 0);
    TIM_SetCompare3(TIM1, __GmLeft.sSpeed);
    break;

  case __MOTORGOBACK:
    TIM_SetCompare4(TIM1, __GmLeft.sSpeed);
    TIM_SetCompare3(TIM1, 0);
    break;

  case __MOTORGOSTOP:
    TIM_SetCompare3(TIM1, 0);
    TIM_SetCompare4(TIM1, 0);
    break;

  default:
    break;
  }
}

/*********************************************************************************************************
** Function name:       __SpeedUp
** Descriptions:        电脑鼠加速程序
** input parameters:    无
** output parameters:   无
** Returned value:      无
*********************************************************************************************************/
void __SpeedUp(void)
{
  uint16 Speed;
  uint16 S;
  Speed = __GmSPID.sFeedBack;
  S = __GiMaxSpeed - __GmSPID.sFeedBack;
  if (__GmSPID.sRef < __GiMaxSpeed)
  {

    if (Speed >= __GmSPID.sRef)
    {
      if (S > 200)
        __GmSPID.sRef = __GmSPID.sRef + 3;
      else if (S > 150)
        __GmSPID.sRef = __GmSPID.sRef + 5;
      else if (S > 100)
        __GmSPID.sRef = __GmSPID.sRef + 8;
      else
        __GmSPID.sRef = __GmSPID.sRef + 10;
    }
  }
}
/*********************************************************************************************************
** Function name:       __SpeedDown
** Descriptions:        电脑鼠减速程序
** input parameters:    无
** output parameters:   无
** Returned value:      无
*********************************************************************************************************/
void __SpeedDown(void)
{
  uint16 Speed;
  uint16 S1;
  Speed = __GmSPID.sFeedBack;
  S1 = __GmSPID.sFeedBack - __GiMinSpeed;

  if (__GmSPID.sRef >= __GiMinSpeed)
  {
    if ((S1 > 250) || (S1 < 100))
    {
      if (Speed <= __GmSPID.sRef + 3)
      {
        __GmSPID.sRef = __GmSPID.sRef - 3;
      }
    }
    else
    {
      if (Speed <= __GmSPID.sRef + 5)
      {
        __GmSPID.sRef = __GmSPID.sRef - 5;
      }
    }
  }
}
/*void __SpeedDown (void)
{
    uint16 Speed;
    uint16 S1;
    Speed=__GmSPID.sFeedBack;
    S1=__GiMaxSpeed-__GmSPID.sFeedBack;
    if(__GmSPID.sRef>=MINSPEED)
    {
      if(S1>200){
         if(Speed <=__GmSPID.sRef+3)
        {
          __GmSPID.sRef=__GmSPID.sRef-3;
        }
      }
      else if(S1>100){
         if(Speed <=__GmSPID.sRef+2)
        {
          __GmSPID.sRef=__GmSPID.sRef-2;
        }
      }
      if(S1>50){
         if(Speed <=__GmSPID.sRef+1)
        {
          __GmSPID.sRef=__GmSPID.sRef-1;
        }
      }
    }
}*/

/*void __SpeedDown (void)
{

    uint16 Speed;
    Speed=__GmSPID.sFeedBack;
    if(__GmSPID.sRef>=MINSPEED){
      if(Speed <=__GmSPID.sRef+2)
      {
       if((Speed-__GiMaxSpeed)>=5)

          __GmSPID.sRef=__GmSPID.sRef-3;

       else

          __GmSPID.sRef=__GmSPID.sRef-2;
      }
    }
}*/
/*********************************************************************************************************
** Function name:       startCheck
** Descriptions:        读取按键
** input parameters:    无
** output parameters:   无
** Returned value:      true:  按键已按下
**                      false: 按键未按下
*********************************************************************************************************/
uint8 startCheck(void)
{
  if (GPIO_ReadInputDataBit(GPIOC, __START) == 0)
  {
    __delay(50);
    while (GPIO_ReadInputDataBit(GPIOC, __START) == 0)
      ;
    return (true);
  }
  else
  {
    return (false);
  }
}

void TIM2_IROHandler(void)
{
  TIM_ClearFlag(TIM2, TIM_FLAG_Update);
}

void TIM3_IRQHandler(void)
{
  TIM_ClearFlag(TIM3, TIM_FLAG_Update);
}

/*********************************************************************************************************
** Function name:       SysTick_Handler
** Descriptions:        定时中断扫描。
** input parameters:    无
** output parameters:   无
** Returned value:      无
*********************************************************************************************************/
void SysTick_Handler(void)
{
  static int8 n = 0, m = 0, k = 0, a = 0, b = 0, c = 0;
  uint16 Sp;
  __Encoder();
  __irCheck();
  Sp = __GmSPID.sFeedBack;

  switch (__GmRight.cState)
  {
  case __MOTORSTOP: /*  停止，同时清零速度和脉冲值  */
    __GmRight.uiPulse = 0;
    __GmRight.uiPulseCtr = 0;
    __GmLeft.uiPulse = 0;
    __GmLeft.uiPulseCtr = 0;
    break;

  case __WAITONESTEP: /*  暂停一步                    */
    __GmRight.cState = __MOTORRUN;
    if ((((ucIRCheck[2] > GusDistance_L_Near) && (ucIRCheck[3] < GusDistance_R_Near)) && (ucIRCheck[3] > GusDistance_R_Mid))) //偏左
    {
      GsTpusle_T = -4;
    }
    else if ((((ucIRCheck[2] < GusDistance_L_Near) && (ucIRCheck[3] > GusDistance_R_Near)) && (ucIRCheck[2] > GusDistance_L_Mid))) //偏右
    {
      GsTpusle_T = 4;
    }
    if ((ucIRCheck[2] > GusDistance_L_Near) && (ucIRCheck[3] < GusDistance_R_Mid)) //偏左
    {
      GsTpusle_T = -6;
    }
    else if ((ucIRCheck[3] > GusDistance_R_Near) && (ucIRCheck[2] < GusDistance_L_Mid)) //偏右
    {
      GsTpusle_T = 6;
    }
    else if ((ucIRCheck[2] < GusDistance_L_Far) && ((ucIRCheck[3] > GusDistance_R_Far) && (ucIRCheck[3] < GusDistance_R_Mid)))
    {
      GsTpusle_T = -5;
    }

    else if ((ucIRCheck[3] < GusDistance_R_Far) && ((ucIRCheck[2] > GusDistance_L_Far) && (ucIRCheck[2] < GusDistance_L_Mid)))
    {
      GsTpusle_T = 5;
    }
    else
      GsTpusle_T = 0;
    __PIDContr();
    break;
    /* case __WAITONESTEP1:
              __GmRight.cState = __MOTORRUN;
              if((((ucIRCheck[2]>GusDistance_L_Near)&&(ucIRCheck[3]<GusDistance_R_Near))&&(ucIRCheck[3]>GusDistance_R_Mid)))          //偏左
              {
                GsTpusle_T = -2;
              }
              else if((((ucIRCheck[2]<GusDistance_L_Near)&&(ucIRCheck[3]>GusDistance_R_Near))&&(ucIRCheck[2]>GusDistance_L_Mid)))     //偏右
              {
                GsTpusle_T = 2;
              }
              if((ucIRCheck[2]>GusDistance_L_Near)&&(ucIRCheck[3]<GusDistance_R_Mid))          //偏左
              {
                GsTpusle_T = -4;
              }
              else if((ucIRCheck[3]>GusDistance_R_Near)&&(ucIRCheck[2]<GusDistance_L_Mid))     //偏右
              {
                GsTpusle_T = 4;
              }
              else if((ucIRCheck[2]<GusDistance_L_Far)&&((ucIRCheck[3]>GusDistance_R_Far)&&(ucIRCheck[3]<GusDistance_R_Mid)))
              {
                  GsTpusle_T = 3;
              }

              else if((ucIRCheck[3]<GusDistance_R_Far)&&((ucIRCheck[2]>GusDista，nce_L_Far)&&(ucIRCheck[2]<GusDistance_L_Mid)))
              {
                   GsTpusle_T = -3;
              }
                else
                  GsTpusle_T = 0;
              __PIDContr();
              break;    */

  case __MOTORRUN:                    /*  电机运行                    */
    if (__GucMouseState == __GOAHEAD) /*  根据传感器状态微调电机位置  */
    {

#pragma region 微调电脑鼠方向
      if ((ucIRCheck[2] > GusDistance_L_Near) && (ucIRCheck[3] < GusDistance_R_Near))
      {
        // 电脑鼠偏右
        if (n == 1)
        {
          __GmRight.cState = __WAITONESTEP;
        }
        n++;
        n %= 2;
      }
      else if ((ucIRCheck[2] < GusDistance_L_Near) && (ucIRCheck[3] > GusDistance_R_Near))
      {
        if (a == 1)
        {
          __GmRight.cState = __WAITONESTEP;
        }
        a++;
        a %= 2;
      }

      else if ((ucIRCheck[2] < GusDistance_L_Mid) && (ucIRCheck[2] > GusDistance_L_Far))
      {
        if (b == 1)
        {
          __GmRight.cState = __WAITONESTEP;
        }
        b++;
        b %= 2;
      }
      else if ((ucIRCheck[3] < GusDistance_R_Mid) && (ucIRCheck[3] > GusDistance_R_Far))
      {
        if (c == 1)
        {
          __GmRight.cState = __WAITONESTEP;
        }
        c++;
        c %= 2;
      }
      else
      {
        m = 0;
        n = 0;
        GsTpusle_T = 0;
      }
#pragma endregion 使左或右轮 1 / 2 间歇运动，从而调整电脑鼠移动方向

#pragma region 微调加速或减速
      if (GuiSpeedCtr == __SPEEDUP)
      {
        k = (k + 1) % 6; //20
        if (k == 2)
          __SpeedUp();
      }
      else if (GuiSpeedCtr == __SPEEDDOWN)
      {
        k = (k + 1) % 3; //10
        if (k == 2)      //9
          __SpeedDown();
      }
      else
        ;
#pragma endregion 做 1 / 6 加速或 1 / 3 减速
    }

    else if (__GucMouseState == __GOBACK) /*  根据传感器状态微调电机位置  */
    {
#pragma region 垃圾注释
      /*if ((ucIRCheck[2]>GusDistance_L_Near)&&(ucIRCheck[3]<GusDistance_R_Near))
                  {
                    if (n == 1)
                    {
                          __GmRight.cState = __WAITONESTEP1;
                    }
                    n++;
                    n %= 2;
                  }
                  else if((ucIRCheck[2]<GusDistance_L_Near)&&(ucIRCheck[3]>GusDistance_R_Near))
                  {
                    if (a == 1)
                    {
                          __GmRight.cState = __WAITONESTEP1;
                    }
                    a++;
                    a %= 2;
                  }

                  else if((ucIRCheck[2]<GusDistance_L_Mid)&&(ucIRCheck[2]>GusDistance_L_Far))
                  {
                    if (b == 1)
                    {
                          __GmRight.cState = __WAITONESTEP1;
                    }
                    b++;
                    b %= 2;
                  }
                  else if((ucIRCheck[3]<GusDistance_R_Mid)&&(ucIRCheck[3]>GusDistance_R_Far))
                  {
                   if (c == 1)
                   {
                          __GmRight.cState = __WAITONESTEP1;
                    }
                    c++;
                    c %= 2;
                  }
                 else
                  {
                      m  = 0;
                      n = 0;
                      GsTpusle_T = 0;
                  }

                  if(GuiSpeedCtr==__SPEEDUP)
                  {
                    k=(k+1)%5;//20
                    if(k==4)
                    __SpeedUp();
                  }
                  else if(GuiSpeedCtr==__SPEEDDOWN)
                  {
                      k=(k+1)%10;
                      if(k==9)
                      __SpeedDown();

                  }
                  else;*/
#pragma endregion
    }

    else
    {
      GsTpusle_T = 0;
      if (__GucMouseState == __TURNRIGHT)
      {

        if (time > 0)
        {
          __GmWPID.sRef = -50;
        }
        time++;
      }

      else if (__GucMouseState == __TURNLEFT) //左转
      {

        if (time >= 0) //163
        {
          __GmWPID.sRef = 50;
        }
        time++;
      }

      else if (__GucMouseState == __TURNRIGHT1)
      {

        if (time > 0)
        {
          __GmWPID.sRef = -60;
        }
        time++;
      }
      else if (__GucMouseState == __TURNLEFT1)
      {

        if (time > 0)
        {
          __GmWPID.sRef = 60;
        }
        time++;
      }

      else if (__GucMouseState == __TURNBACK)
      {
        // if (backflag == 1)
        // {
        //   backflag = 0;
        //   if (time >= 0)
        //     __GmWPID.sRef = -40;

        //   time++;
        // }

        // else
        // {
        if (time >= 0)
          __GmWPID.sRef = -40;
        time++;
        // }
      }

#pragma region 删除无用代码
// else if (__GucMouseState == __GOBACK)
//   ;
#pragma endregion
    }
    __PIDContr();
    break;
  case 4: // 未定义的右电机状态，也无赋值
    GsTpusle_T = 0;
    __PIDContr();
    break;

  case 5: // 未定义的右电机状态，也无赋值
    __GmRight.sSpeed = 10;
    __rightMotorContr();
    __GmRight.cState = __MOTORSTOP;
    __GmLeft.sSpeed = 10;
    __leftMotorContr();
    __GmLeft.cState = __MOTORSTOP;
    break;

  default:
    break;
  }
}

/*********************************************************************************************************
** Function name:       mazeSearch
** Descriptions:        前进N格
** input parameters:    iNblock: 前进的格数
** output parameters:   无
** Returned value:      无
*********************************************************************************************************/

void mouseGoaheadhui(int8 cNBlock) //电脑鼠原路返回时程序
{
  int8 cL = 0, cR = 0, cCoor = 1, cB;
  GuiSpeedCtr = __SPEEDUP;
  if (__GmLeft.cState)
  {
    cCoor = 0;
  }
  if (cNBlock == 1)
  {
    cL = 1;
    cR = 1;

    if (GucFangXiang == GucDirTemp)
    {

      GuiTpusle_LR = 15000;
      GuiTpusle_S = 0;
    }
    else
    {
      GuiTpusle_LR = 10000;
      GuiTpusle_S = 600;
    }
    __GiMaxSpeed = 110;
  }
  else
  {
    GuiTpusle_LR = 0;
    GuiTpusle_S = 0;
    __GiMaxSpeed = 250;
  }
  GucFangXiang = GucDirTemp;
  if (((GmcMouse.cX == 7) && (GmcMouse.cY == 7)) ||
      ((GmcMouse.cX == 8) && (GmcMouse.cY == 8)) ||
      ((GmcMouse.cX == 7) && (GmcMouse.cY == 8)) ||
      ((GmcMouse.cX == 8) && (GmcMouse.cY == 7)))
  {
    cL = 0;
    cR = 0;
    GuiTpusle_LR = 0;
    GuiTpusle_S = 0;
  }
  if ((__GucMouseState == __TURNRIGHT) || (__GucMouseState == __TURNLEFT))
  {
    __GmLeft.uiPulseCtr = 30000; //1182(34mm)
    __GmRight.uiPulseCtr = 30000;
  }

  cB = cNBlock;
  __GucMouseState = __GOAHEAD;

  __GmRight.uiPulse = __GmRight.uiPulse + cNBlock * ONEBLOCK;
  __GmLeft.uiPulse = __GmLeft.uiPulse + cNBlock * ONEBLOCK;
  __GmRight.cState = __MOTORRUN;
  __GmLeft.cState = __MOTORRUN;
  if (cNBlock > 7)
  {
    __GiMaxSpeed = 250; //300//320
  }

  else if (cNBlock == 7)
  {
    __GiMaxSpeed = 240; //250//270
  }
  else if (cNBlock == 6)
  {
    __GiMaxSpeed = 220; //250//270
  }

  else if (cNBlock == 5)
  {
    __GiMaxSpeed = 200; //250//270
  }
  else if (cNBlock == 4)
  {
    __GiMaxSpeed = 160; //200//220
  }
  else if (cNBlock == 3)
  {
    __GiMaxSpeed = 120; //180//200
  }
  else if (cNBlock == 2)
  {
    __GiMaxSpeed = 110; //150//160
  }
  else
    ;

  while (__GmLeft.cState != __MOTORSTOP)
  {

    if (__GmLeft.uiPulseCtr >= ONEBLOCK)
    { /*  判断是否走完一格            */
      __GmLeft.uiPulse -= ONEBLOCK;
      __GmLeft.uiPulseCtr -= ONEBLOCK;
      if (cCoor)
      {
        cNBlock--;
        if (cNBlock == 0)
          goto End;
        if (cNBlock < cB - 1) //给回速一个时间
          GuiSpeedCtr = __SPEEDUP;
      }
      else
      {
        cCoor = 1;
      }
    }

    if (__GmRight.uiPulseCtr >= ONEBLOCK)
    { /*  判断是否走完一格            */
      __GmRight.uiPulse -= ONEBLOCK;
      __GmRight.uiPulseCtr -= ONEBLOCK;
    }
    if (cNBlock < 3)
    {
      if (__GmSPID.sFeedBack > 110)
      {
        __GiMinSpeed = 110;
        GuiSpeedCtr = __SPEEDDOWN;
      }
    }
    if (cNBlock < 2)
    {

      if (__GmSPID.sFeedBack > 100)
      {
        GuiSpeedCtr = 3;
        __GmSPID.sRef = 100;
      }

      if (cL)
      {                                       /*  是否允许检测左边            */
        if (ucIRCheck[2] < GusDistance_L_Far) /*  左边有支路，跳出程序        */
        {
          __GmRight.uiPulse = __GmRight.uiPulseCtr + 19000 - GuiTpusle_LR; //电脑鼠返回时左转之前20100
          __GmLeft.uiPulse = __GmLeft.uiPulseCtr + 19000 - GuiTpusle_LR;   //电脑鼠返回时左转之前20100
                                                                           // mouseStop();
                                                                           //while(1);

          while (ucIRCheck[2] < GusDistance_L_Far)
          {

            if ((__GmLeft.uiPulseCtr + 100) > __GmLeft.uiPulse)
            {
              goto End;
            }
          }
        }
      }
      else
      { /*  左边有墙时开始允许检测左边  */
        if (ucIRCheck[2] > GusDistance_L_Far)
        {
          cL = 1;
        }
      }
      if (cR)
      {                                       /*  是否允许检测右边            */
        if (ucIRCheck[3] < GusDistance_R_Far) /*  右边有支路，跳出程序        */
        {
          __GmRight.uiPulse = __GmRight.uiPulseCtr + 19000 - GuiTpusle_LR; //电脑鼠返回时右转之前20200
          __GmLeft.uiPulse = __GmLeft.uiPulseCtr + 19000 - GuiTpusle_LR;   //电脑鼠返回时右转之前20200
          //mouseStop();
          // while(1);

          while (ucIRCheck[3] < GusDistance_R_Far)
          {

            if ((__GmLeft.uiPulseCtr + 100) > __GmLeft.uiPulse)
            {
              goto End;
            }
          }
        }
      }
      else
      {
        if (ucIRCheck[3] > GusDistance_R_Far)
        { /*  右边有墙时开始允许检测右边  */
          cR = 1;
        }
      }
    }
  }
  /*
     *  设定运行任务，让电脑鼠走到支路的中心位置
     */
End:
}

void mouseGoaheadhui_Low(int8 cNBlock) //电脑鼠搜索返回连续转弯用
{
  int8 cL = 0, cR = 0, cCoor = 1, cB;
  GuiSpeedCtr = __SPEEDUP;
  if (__GmLeft.cState)
  {
    cCoor = 0;
  }
  if (cNBlock == 1)
  {
    cL = 1;
    cR = 1;
    //GuiTpusle_LR = 0;    //100
    if (GucFangXiang == GucDirTemp)
    {
      //GucYiBaiBa=1;
      GuiTpusle_LR = 15000;
      GuiTpusle_S = 0;
    }
    else
    {
      GuiTpusle_LR = 10000;
      GuiTpusle_S = 600;
    }
    __GiMaxSpeed = 100;
  }
  else
  {
    GuiTpusle_LR = 0;
    GuiTpusle_S = 0;
    __GiMaxSpeed = 100;
  }
  GucFangXiang = GucDirTemp;
  if (((GmcMouse.cX == 7) && (GmcMouse.cY == 7)) ||
      ((GmcMouse.cX == 8) && (GmcMouse.cY == 8)) ||
      ((GmcMouse.cX == 7) && (GmcMouse.cY == 8)) ||
      ((GmcMouse.cX == 8) && (GmcMouse.cY == 7)))
  {
    cL = 0;
    cR = 0;
    GuiTpusle_LR = 0;
    GuiTpusle_S = 0;
  }
  if ((__GucMouseState == __TURNRIGHT) || (__GucMouseState == __TURNLEFT))
  {
    __GmLeft.uiPulseCtr = 30000; //1182(34mm)
    __GmRight.uiPulseCtr = 30000;
  }

  cB = cNBlock;
  __GucMouseState = __GOAHEAD;

  __GmRight.uiPulse = __GmRight.uiPulse + cNBlock * ONEBLOCK;
  __GmLeft.uiPulse = __GmLeft.uiPulse + cNBlock * ONEBLOCK;
  __GmRight.cState = __MOTORRUN;
  __GmLeft.cState = __MOTORRUN;

  while (__GmLeft.cState != __MOTORSTOP)
  {

    if (__GmLeft.uiPulseCtr >= ONEBLOCK)
    { /*  判断是否走完一格            */
      __GmLeft.uiPulse -= ONEBLOCK;
      __GmLeft.uiPulseCtr -= ONEBLOCK;
      if (cCoor)
      {
        cNBlock--;
        if (cNBlock == 0)
          goto End;
        if (cNBlock < cB - 1) //给回速一个时间
          GuiSpeedCtr = __SPEEDUP;
      }
      else
      {
        cCoor = 1;
      }
    }

    if (__GmRight.uiPulseCtr >= ONEBLOCK)
    { /*  判断是否走完一格            */
      __GmRight.uiPulse -= ONEBLOCK;
      __GmRight.uiPulseCtr -= ONEBLOCK;
    }
    if (cNBlock < 3)
    {
      if (__GmSPID.sFeedBack > 110)
      {
        __GiMinSpeed = 110;
        GuiSpeedCtr = __SPEEDDOWN;
      }
    }
    if (cNBlock < 2)
    {

      if (__GmSPID.sFeedBack > 100)
      {
        GuiSpeedCtr = 3;
        __GmSPID.sRef = 100;
      }

      if (cL)
      {                                       /*  是否允许检测左边            */
        if (ucIRCheck[2] < GusDistance_L_Far) /*  左边有支路，跳出程序        */
        {
          __GmRight.uiPulse = __GmRight.uiPulseCtr + 15200 - GuiTpusle_LR; //3094(89mm)22000
          __GmLeft.uiPulse = __GmLeft.uiPulseCtr + 15200 - GuiTpusle_LR;   //22000
                                                                           // mouseStop();
                                                                           //while(1);

          while (ucIRCheck[2] < GusDistance_L_Far)
          {

            if ((__GmLeft.uiPulseCtr + 100) > __GmLeft.uiPulse)
            {
              goto End;
            }
          }
        }
      }
      else
      { /*  左边有墙时开始允许检测左边  */
        if (ucIRCheck[2] > GusDistance_L_Far)
        {
          cL = 1;
        }
      }
      if (cR)
      {                                       /*  是否允许检测右边            */
        if (ucIRCheck[3] < GusDistance_R_Far) /*  右边有支路，跳出程序        */
        {
          __GmRight.uiPulse = __GmRight.uiPulseCtr + 15000 - GuiTpusle_LR; //22000
          __GmLeft.uiPulse = __GmLeft.uiPulseCtr + 15000 - GuiTpusle_LR;   //22000

          //mouseStop();
          //while(1);

          while (ucIRCheck[3] < GusDistance_R_Far)
          {

            if ((__GmLeft.uiPulseCtr + 100) > __GmLeft.uiPulse)
            {
              goto End;
            }
          }
        }
      }
      else
      {
        if (ucIRCheck[3] > GusDistance_R_Far)
        { /*  右边有墙时开始允许检测右边  */
          cR = 1;
        }
      }
    }
  }
  /*
     *  设定运行任务，让电脑鼠走到支路的中心位置
     */
End:; //GuiSpeedCtr= 3;
      //__GmSPID.sRef=23;
}

void mouseTurnrighthui(void) //电脑鼠搜索返回右转
{

  GW = 0;
  time = 0;

  __GmWPID.sRef = -50; //转弯角速度
  __GmSPID.sRef = 100; //转弯直线速度

  __GucMouseState = __TURNRIGHT;

  __GmRight.cState = __MOTORRUN; //得加电脑鼠状态
  __GmLeft.cState = __MOTORRUN;
  GucMouseDir = (GucMouseDir + 1) % 4; /*  方向标记                    */

  while (1)
  {
    if (time > 300) //290
      break;
  }

  __GmWPID.sRef = 0;
  __GucMouseState = __GOAHEAD;
  GuiSpeedCtr = 3;
  __GmLeft.uiPulse = 5200; //转弯之后距离，调节连续转弯用  5300
  __GmLeft.uiPulseCtr = 0;
  __GmRight.uiPulse = 5200; //转弯之后距离，调节连续转弯用     5300
  __GmRight.uiPulseCtr = 0;
  while ((__GmRight.uiPulseCtr + 200) <= __GmRight.uiPulse)
    ;
  while ((__GmLeft.uiPulseCtr + 200) <= __GmLeft.uiPulse)
    ;
  __GucMouseState = __TURNRIGHT;
  GuiSpeedCtr = __SPEEDUP;
  __GmRight.cState = __MOTORSTOP;
  __GmLeft.cState = __MOTORSTOP;
  __GmRight.sSpeed = 0;
  __rightMotorContr();
  __GmLeft.sSpeed = 0;
  __leftMotorContr();
  __GmRight.uiPulseCtr = 10000;
  __GmLeft.uiPulseCtr = 10000;
  // mouseStop();
  // while(1);
}
void mouseTurnlefthui(void) //电脑鼠搜索返回左转
{

  GW = 0;
  time = 0;
  __GmWPID.sRef = 50;  //转弯角速度
  __GmSPID.sRef = 100; //转弯直线速度
  __GucMouseState = __TURNLEFT;
  __GmRight.cState = __MOTORRUN; //得加电脑鼠状态
  __GmLeft.cState = __MOTORRUN;
  GucMouseDir = (GucMouseDir + 3) % 4; /*  方向标记                    */
  while (1)
  {
    if (time > 300) //292
      break;
  }
  __GmWPID.sRef = 0;
  __GucMouseState = __GOAHEAD;

  //mouseStop();
  //while(1);
  GuiSpeedCtr = 3;
  __GmLeft.uiPulse = 6000; //转弯之后距离，调节连续转弯用  4900
  __GmLeft.uiPulseCtr = 0;
  __GmRight.uiPulse = 6000; //转弯之后距离，调节连续转弯用     4900
  __GmRight.uiPulseCtr = 0;
  while ((__GmRight.uiPulseCtr + 200) <= __GmRight.uiPulse)
    ;
  while ((__GmLeft.uiPulseCtr + 200) <= __GmLeft.uiPulse)
    ;
  __GucMouseState = __TURNLEFT;
  GuiSpeedCtr = __SPEEDUP;
  __GmRight.cState = __MOTORSTOP;
  __GmLeft.cState = __MOTORSTOP;
  __GmRight.sSpeed = 0;
  __rightMotorContr();
  __GmLeft.sSpeed = 0;
  __leftMotorContr();
  __GmRight.uiPulseCtr = 10000;
  __GmLeft.uiPulseCtr = 10000;
  //mouseStop();
  //while(1);
}

void GoAhead(float speedRef, uint32 pulse)
{
  __GmSPID.sRef = speedRef;
  __GmRight.uiPulse = pulse;
  __GmRight.uiPulseCtr = 0;
  __GmLeft.uiPulse = pulse;
  __GmLeft.uiPulseCtr = 0;
}

// 等待运动临近结束
void WaitGoOver()
{
  while ((__GmRight.uiPulseCtr + 200) <= __GmRight.uiPulse ||
         (__GmLeft.uiPulseCtr + 200) <= __GmLeft.uiPulse)
    ;
}

void mouseGoahead1(int8 cNBlock) //冲刺看前方红外，无坐标更新,当goto end速度大于20时设定速度为20
{
  int8 cL = 0, cR = 0, cCoor = 1, cB;
  GuiSpeedCtr = __SPEEDUP;
  if (__GmLeft.cState)
  {
    cCoor = 0;
  }
  if (cNBlock == 1)
  {
    cL = 1;
    cR = 1;

    if (GucFangXiang == GucDirTemp)
    {

      GuiTpusle_LR = 15000;
      GuiTpusle_S = 0;
    }
    else
    {
      GuiTpusle_LR = 10000;
      GuiTpusle_S = 600;
    }
    __GiMaxSpeed = 150;
  }
  else
  {
    GuiTpusle_LR = 0;
    GuiTpusle_S = 0;
    __GiMaxSpeed = 350;
  }
  GucFangXiang = GucDirTemp;
  if (((GmcMouse.cX == 7) && (GmcMouse.cY == 7)) ||
      ((GmcMouse.cX == 8) && (GmcMouse.cY == 8)) ||
      ((GmcMouse.cX == 7) && (GmcMouse.cY == 8)) ||
      ((GmcMouse.cX == 8) && (GmcMouse.cY == 7)))
  {
    cL = 0;
    cR = 0;
    GuiTpusle_LR = 0;
    GuiTpusle_S = 0;
  }
  if ((__GucMouseState == __TURNRIGHT) || (__GucMouseState == __TURNLEFT))
  {
    __GmLeft.uiPulseCtr = 30000; //1182(34mm)
    __GmRight.uiPulseCtr = 30000;
  }

  cB = cNBlock;
  __GucMouseState = __GOAHEAD;

  __GmRight.uiPulse = __GmRight.uiPulse + cNBlock * ONEBLOCK;
  __GmLeft.uiPulse = __GmLeft.uiPulse + cNBlock * ONEBLOCK;
  __GmRight.cState = __MOTORRUN;
  __GmLeft.cState = __MOTORRUN;
  if (cNBlock > 7)
  {
    __GiMaxSpeed = 350; //300//320
  }

  else if (cNBlock == 7)
  {
    __GiMaxSpeed = 350; //250//270
  }
  else if (cNBlock == 6)
  {
    __GiMaxSpeed = 330; //250//270
  }

  else if (cNBlock == 5)
  {
    __GiMaxSpeed = 300; //250//270
  }
  else if (cNBlock == 4)
  {
    __GiMaxSpeed = 240; //200//220
  }
  else if (cNBlock == 3)
  {
    __GiMaxSpeed = 180; //180//200
  }
  else if (cNBlock == 2)
  {
    __GiMaxSpeed = 150; //150//160
  }
  else
    ;

  while (__GmLeft.cState != __MOTORSTOP)
  {

    if (__GmLeft.uiPulseCtr >= ONEBLOCK)
    { /*  判断是否走完一格            */
      __GmLeft.uiPulse -= ONEBLOCK;
      __GmLeft.uiPulseCtr -= ONEBLOCK;
      if (cCoor)
      {
        cNBlock--;
        if (cNBlock == 0)
          goto End;
        if (cNBlock < cB - 1) //给回速一个时间
          GuiSpeedCtr = __SPEEDUP;
      }
      else
      {
        cCoor = 1;
      }
    }

    if (__GmRight.uiPulseCtr >= ONEBLOCK)
    { /*  判断是否走完一格            */
      __GmRight.uiPulse -= ONEBLOCK;
      __GmRight.uiPulseCtr -= ONEBLOCK;
    }
    if (cNBlock < 3)
    {
      if (__GmSPID.sFeedBack > 150)
      {
        __GiMinSpeed = 150;
        GuiSpeedCtr = __SPEEDDOWN;
      }
    }
    if (cNBlock < 2)
    {

      if (__GmSPID.sFeedBack > 120)
      {
        GuiSpeedCtr = 3;
        __GmSPID.sRef = 120;
      }

      if (cL)
      {                                       /*  是否允许检测左边            */
        if (ucIRCheck[2] < GusDistance_L_Far) /*  左边有支路，跳出程序        */
        {
          __GmRight.uiPulse = __GmRight.uiPulseCtr + 17000 - GuiTpusle_LR; //不动18000
          __GmLeft.uiPulse = __GmLeft.uiPulseCtr + 17000 - GuiTpusle_LR;   //
          while (ucIRCheck[2] < GusDistance_L_Far)

          {

            if ((__GmLeft.uiPulseCtr + 100) > __GmLeft.uiPulse)
            {
              goto End;
            }
          }
        }
      }
      else
      { /*  左边有墙时开始允许检测左边  */
        if (ucIRCheck[2] > GusDistance_L_Far)
        {
          cL = 1;
        }
      }
      if (cR)
      {                                       /*  是否允许检测右边            */
        if (ucIRCheck[3] < GusDistance_R_Far) /*  右边有支路，跳出程序        */
        {
          __GmRight.uiPulse = __GmRight.uiPulseCtr + 17000 - GuiTpusle_LR; //15000不动
          __GmLeft.uiPulse = __GmLeft.uiPulseCtr + 17000 - GuiTpusle_LR;   //15000
          while (ucIRCheck[3] < GusDistance_R_Far)
          {

            if ((__GmLeft.uiPulseCtr + 100) > __GmLeft.uiPulse)
            {
              goto End;
            }
          }
        }
      }
      else
      {
        if (ucIRCheck[3] > GusDistance_R_Far)
        { /*  右边有墙时开始允许检测右边  */
          cR = 1;
        }
      }
    }
  }
  /*
     *  设定运行任务，让电脑鼠走到支路的中心位置
     */
End:
}

void mouseGoahead1_Low(int8 cNBlock) //连续转弯用
{
  int8 cL = 0, cR = 0, cCoor = 1, cB;
  GuiSpeedCtr = __SPEEDUP;
  if (__GmLeft.cState)
  {
    cCoor = 0;
  }
  if (cNBlock == 1)
  {
    cL = 1;
    cR = 1;

    if (GucFangXiang == GucDirTemp)
    {

      GuiTpusle_LR = 15000;
      GuiTpusle_S = 0;
    }
    else
    {
      GuiTpusle_LR = 10000;
      GuiTpusle_S = 600;
    }
    __GiMaxSpeed = 120;
  }
  else
  {
    GuiTpusle_LR = 0;
    GuiTpusle_S = 0;
    __GiMaxSpeed = 120;
  }
  GucFangXiang = GucDirTemp;
  if (((GmcMouse.cX == 7) && (GmcMouse.cY == 7)) ||
      ((GmcMouse.cX == 8) && (GmcMouse.cY == 8)) ||
      ((GmcMouse.cX == 7) && (GmcMouse.cY == 8)) ||
      ((GmcMouse.cX == 8) && (GmcMouse.cY == 7)))
  {
    cL = 0;
    cR = 0;
    GuiTpusle_LR = 0;
    GuiTpusle_S = 0;
  }
  if ((__GucMouseState == __TURNRIGHT) || (__GucMouseState == __TURNLEFT))
  {
    __GmLeft.uiPulseCtr = 30000; //1182(34mm)
    __GmRight.uiPulseCtr = 30000;
  }

  cB = cNBlock;
  __GucMouseState = __GOAHEAD;

  __GmRight.uiPulse = __GmRight.uiPulse + cNBlock * ONEBLOCK;
  __GmLeft.uiPulse = __GmLeft.uiPulse + cNBlock * ONEBLOCK;
  __GmRight.cState = __MOTORRUN;
  __GmLeft.cState = __MOTORRUN;

  while (__GmLeft.cState != __MOTORSTOP)
  {

    if (__GmLeft.uiPulseCtr >= ONEBLOCK)
    { /*  判断是否走完一格            */
      __GmLeft.uiPulse -= ONEBLOCK;
      __GmLeft.uiPulseCtr -= ONEBLOCK;
      if (cCoor)
      {
        cNBlock--;
        if (cNBlock == 0)
          goto End;
        if (cNBlock < cB - 1) //给回速一个时间
          GuiSpeedCtr = __SPEEDUP;
      }
      else
      {
        cCoor = 1;
      }
    }

    if (__GmRight.uiPulseCtr >= ONEBLOCK)
    { /*  判断是否走完一格            */
      __GmRight.uiPulse -= ONEBLOCK;
      __GmRight.uiPulseCtr -= ONEBLOCK;
    }

    if (cNBlock < 2)
    {

      if (__GmSPID.sFeedBack > 120)
      {
        GuiSpeedCtr = 3;
        __GmSPID.sRef = 120;
      }

      if (cL)
      {                                       /*  是否允许检测左边            */
        if (ucIRCheck[2] < GusDistance_L_Far) /*  左边有支路，跳出程序        */
        {
          __GmRight.uiPulse = __GmRight.uiPulseCtr + 13000 - GuiTpusle_LR; //不动18000
          __GmLeft.uiPulse = __GmLeft.uiPulseCtr + 13000 - GuiTpusle_LR;   //
                                                                           // mouseStop();
                                                                           // while(1);

          while (ucIRCheck[2] < GusDistance_L_Far)
          {

            if ((__GmLeft.uiPulseCtr + 100) > __GmLeft.uiPulse)
            {
              goto End;
            }
          }
        }
      }
      else
      { /*  左边有墙时开始允许检测左边  */
        if (ucIRCheck[2] > GusDistance_L_Far)
        {
          cL = 1;
        }
      }
      if (cR)
      {                                       /*  是否允许检测右边            */
        if (ucIRCheck[3] < GusDistance_R_Far) /*  右边有支路，跳出程序        */
        {
          __GmRight.uiPulse = __GmRight.uiPulseCtr + 9000 - GuiTpusle_LR; //不动18500
          __GmLeft.uiPulse = __GmLeft.uiPulseCtr + 9000 - GuiTpusle_LR;

          while (ucIRCheck[3] < GusDistance_R_Far)
          {

            if ((__GmLeft.uiPulseCtr + 100) > __GmLeft.uiPulse)
            {
              goto End;
            }
          }
        }
      }
      else
      {
        if (ucIRCheck[3] > GusDistance_R_Far)
        { /*  右边有墙时开始允许检测右边  */
          cR = 1;
        }
      }
    }
  }
  /*
     *  设定运行任务，让电脑鼠走到支路的中心位置
     */
End:; //GuiSpeedCtr= 3;
      //__GmSPID.sRef=23;
}

//以下是二次冲刺
void mouseGoahead2(int8 cNBlock) //冲刺看前方红外，无坐标更新,当goto end速度大于20时设定速度为20
{
  int8 cL = 0, cR = 0, cCoor = 1, cB;
  GuiSpeedCtr = __SPEEDUP;
  if (__GmLeft.cState)
  {
    cCoor = 0;
  }
  if (cNBlock == 1)
  {
    cL = 1;
    cR = 1;

    if (GucFangXiang == GucDirTemp)
    {

      GuiTpusle_LR = 15000;
      GuiTpusle_S = 0;
    }
    else
    {
      GuiTpusle_LR = 10000;
      GuiTpusle_S = 600;
    }
    __GiMaxSpeed = 160;
  }
  else
  {
    GuiTpusle_LR = 0;
    GuiTpusle_S = 0;
    __GiMaxSpeed = 350;
  }
  GucFangXiang = GucDirTemp;
  if (((GmcMouse.cX == 7) && (GmcMouse.cY == 7)) ||
      ((GmcMouse.cX == 8) && (GmcMouse.cY == 8)) ||
      ((GmcMouse.cX == 7) && (GmcMouse.cY == 8)) ||
      ((GmcMouse.cX == 8) && (GmcMouse.cY == 7)))
  {
    cL = 0;
    cR = 0;
    GuiTpusle_LR = 0;
    GuiTpusle_S = 0;
  }
  if ((__GucMouseState == __TURNRIGHT) || (__GucMouseState == __TURNLEFT))
  {
    __GmLeft.uiPulseCtr = 30000; //1182(34mm)
    __GmRight.uiPulseCtr = 30000;
  }

  cB = cNBlock;
  __GucMouseState = __GOAHEAD;

  __GmRight.uiPulse = __GmRight.uiPulse + cNBlock * ONEBLOCK;
  __GmLeft.uiPulse = __GmLeft.uiPulse + cNBlock * ONEBLOCK;
  __GmRight.cState = __MOTORRUN;
  __GmLeft.cState = __MOTORRUN;
  if (cNBlock > 7)
  {
    __GiMaxSpeed = 350; //300//320
  }

  else if (cNBlock == 7)
  {
    __GiMaxSpeed = 350; //250//270
  }
  else if (cNBlock == 6)
  {
    __GiMaxSpeed = 330; //250//270
  }

  else if (cNBlock == 5)
  {
    __GiMaxSpeed = 320; //250//270
  }
  else if (cNBlock == 4)
  {
    __GiMaxSpeed = 320; //200//220
  }
  else if (cNBlock == 3)
  {
    __GiMaxSpeed = 240; //180//200
  }
  else if (cNBlock == 2)
  {
    __GiMaxSpeed = 160; //150//160
  }
  else
    ;

  while (__GmLeft.cState != __MOTORSTOP)
  {

    if (__GmLeft.uiPulseCtr >= ONEBLOCK)
    { /*  判断是否走完一格            */
      __GmLeft.uiPulse -= ONEBLOCK;
      __GmLeft.uiPulseCtr -= ONEBLOCK;
      if (cCoor)
      {
        cNBlock--;
        if (cNBlock == 0)
          goto End;
        if (cNBlock < cB - 1) //给回速一个时间
          GuiSpeedCtr = __SPEEDUP;
      }
      else
      {
        cCoor = 1;
      }
    }

    if (__GmRight.uiPulseCtr >= ONEBLOCK)
    { /*  判断是否走完一格            */
      __GmRight.uiPulse -= ONEBLOCK;
      __GmRight.uiPulseCtr -= ONEBLOCK;
    }
    if (cNBlock < 3)
    {
      if (__GmSPID.sFeedBack > 160)
      {
        __GiMinSpeed = 160;
        GuiSpeedCtr = __SPEEDDOWN;
      }
    }
    if (cNBlock < 2)
    {

      if (__GmSPID.sFeedBack > 120)
      {
        GuiSpeedCtr = 3;
        __GmSPID.sRef = 120;
      }

      if (cL)
      {                                       /*  是否允许检测左边            */
        if (ucIRCheck[2] < GusDistance_L_Far) /*  左边有支路，跳出程序        */
        {
          __GmRight.uiPulse = __GmRight.uiPulseCtr + 17000 - GuiTpusle_LR; //不动18000
          __GmLeft.uiPulse = __GmLeft.uiPulseCtr + 17000 - GuiTpusle_LR;   //
          while (ucIRCheck[2] < GusDistance_L_Far)

          {

            if ((__GmLeft.uiPulseCtr + 100) > __GmLeft.uiPulse)
            {
              goto End;
            }
          }
        }
      }
      else
      { /*  左边有墙时开始允许检测左边  */
        if (ucIRCheck[2] > GusDistance_L_Far)
        {
          cL = 1;
        }
      }
      if (cR)
      {                                       /*  是否允许检测右边            */
        if (ucIRCheck[3] < GusDistance_R_Far) /*  右边有支路，跳出程序        */
        {
          __GmRight.uiPulse = __GmRight.uiPulseCtr + 17000 - GuiTpusle_LR; //15000不动
          __GmLeft.uiPulse = __GmLeft.uiPulseCtr + 17000 - GuiTpusle_LR;   //15000
          while (ucIRCheck[3] < GusDistance_R_Far)
          {

            if ((__GmLeft.uiPulseCtr + 100) > __GmLeft.uiPulse)
            {
              goto End;
            }
          }
        }
      }
      else
      {
        if (ucIRCheck[3] > GusDistance_R_Far)
        { /*  右边有墙时开始允许检测右边  */
          cR = 1;
        }
      }
    }
  }
  /*
     *  设定运行任务，让电脑鼠走到支路的中心位置
     */
End:
}

void mouseGoahead2_Low(int8 cNBlock) //连续转弯用
{
  int8 cL = 0, cR = 0, cCoor = 1, cB;
  GuiSpeedCtr = __SPEEDUP;
  if (__GmLeft.cState)
  {
    cCoor = 0;
  }
  if (cNBlock == 1)
  {
    cL = 1;
    cR = 1;

    if (GucFangXiang == GucDirTemp)
    {

      GuiTpusle_LR = 15000;
      GuiTpusle_S = 0;
    }
    else
    {
      GuiTpusle_LR = 10000;
      GuiTpusle_S = 600;
    }
    __GiMaxSpeed = 120;
  }
  else
  {
    GuiTpusle_LR = 0;
    GuiTpusle_S = 0;
    __GiMaxSpeed = 120;
  }
  GucFangXiang = GucDirTemp;
  if (((GmcMouse.cX == 7) && (GmcMouse.cY == 7)) ||
      ((GmcMouse.cX == 8) && (GmcMouse.cY == 8)) ||
      ((GmcMouse.cX == 7) && (GmcMouse.cY == 8)) ||
      ((GmcMouse.cX == 8) && (GmcMouse.cY == 7)))
  {
    cL = 0;
    cR = 0;
    GuiTpusle_LR = 0;
    GuiTpusle_S = 0;
  }
  if ((__GucMouseState == __TURNRIGHT) || (__GucMouseState == __TURNLEFT))
  {
    __GmLeft.uiPulseCtr = 30000; //1182(34mm)
    __GmRight.uiPulseCtr = 30000;
  }

  cB = cNBlock;
  __GucMouseState = __GOAHEAD;

  __GmRight.uiPulse = __GmRight.uiPulse + cNBlock * ONEBLOCK;
  __GmLeft.uiPulse = __GmLeft.uiPulse + cNBlock * ONEBLOCK;
  __GmRight.cState = __MOTORRUN;
  __GmLeft.cState = __MOTORRUN;

  while (__GmLeft.cState != __MOTORSTOP)
  {

    if (__GmLeft.uiPulseCtr >= ONEBLOCK)
    { /*  判断是否走完一格            */
      __GmLeft.uiPulse -= ONEBLOCK;
      __GmLeft.uiPulseCtr -= ONEBLOCK;
      if (cCoor)
      {
        cNBlock--;
        if (cNBlock == 0)
          goto End;
        if (cNBlock < cB - 1) //给回速一个时间
          GuiSpeedCtr = __SPEEDUP;
      }
      else
      {
        cCoor = 1;
      }
    }

    if (__GmRight.uiPulseCtr >= ONEBLOCK)
    { /*  判断是否走完一格            */
      __GmRight.uiPulse -= ONEBLOCK;
      __GmRight.uiPulseCtr -= ONEBLOCK;
    }

    if (cNBlock < 2)
    {

      if (__GmSPID.sFeedBack > 120)
      {
        GuiSpeedCtr = 3;
        __GmSPID.sRef = 120;
      }

      if (cL)
      {                                       /*  是否允许检测左边            */
        if (ucIRCheck[2] < GusDistance_L_Far) /*  左边有支路，跳出程序        */
        {
          __GmRight.uiPulse = __GmRight.uiPulseCtr + 13000 - GuiTpusle_LR; //不动18000
          __GmLeft.uiPulse = __GmLeft.uiPulseCtr + 13000 - GuiTpusle_LR;   //
                                                                           // mouseStop();
                                                                           // while(1);

          while (ucIRCheck[2] < GusDistance_L_Far)
          {

            if ((__GmLeft.uiPulseCtr + 100) > __GmLeft.uiPulse)
            {
              goto End;
            }
          }
        }
      }
      else
      { /*  左边有墙时开始允许检测左边  */
        if (ucIRCheck[2] > GusDistance_L_Far)
        {
          cL = 1;
        }
      }
      if (cR)
      {                                       /*  是否允许检测右边            */
        if (ucIRCheck[3] < GusDistance_R_Far) /*  右边有支路，跳出程序        */
        {
          __GmRight.uiPulse = __GmRight.uiPulseCtr + 9000 - GuiTpusle_LR; //不动18500
          __GmLeft.uiPulse = __GmLeft.uiPulseCtr + 9000 - GuiTpusle_LR;

          while (ucIRCheck[3] < GusDistance_R_Far)
          {

            if ((__GmLeft.uiPulseCtr + 100) > __GmLeft.uiPulse)
            {
              goto End;
            }
          }
        }
      }
      else
      {
        if (ucIRCheck[3] > GusDistance_R_Far)
        { /*  右边有墙时开始允许检测右边  */
          cR = 1;
        }
      }
    }
  }
  /*
     *  设定运行任务，让电脑鼠走到支路的中心位置
     */
End:; //GuiSpeedCtr= 3;
      //__GmSPID.sRef=23;
}

void mouseTurnright1(void) //第一次冲刺右转用
{

  GW = 0;
  time = 0;
  __GmWPID.sRef = -55; //转弯角速度
  __GmSPID.sRef = 110; //转弯直线速度

  __GucMouseState = __TURNRIGHT;

  __GmRight.cState = __MOTORRUN; //得加电脑鼠状态
  __GmLeft.cState = __MOTORRUN;
  GucMouseDir = (GucMouseDir + 1) % 4; /*  方向标记                    */
  while (1)
  {
    if (time > 280) //转弯角度调节290
      break;
  }

  __GmWPID.sRef = 0;
  __GucMouseState = __GOAHEAD;
  // mouseStop();
  // while(1);

  GuiSpeedCtr = 3;
  __GmLeft.uiPulse = 4000; //转弯之后，连续转弯调节用      5000  6300
  __GmLeft.uiPulseCtr = 0;
  __GmRight.uiPulse = 4000; //转弯之后，连续转弯调节用        5000  不动
  __GmRight.uiPulseCtr = 0;
  while ((__GmRight.uiPulseCtr + 200) <= __GmRight.uiPulse)
    ;
  while ((__GmLeft.uiPulseCtr + 200) <= __GmLeft.uiPulse)
    ;
  __GucMouseState = __TURNRIGHT;
  GuiSpeedCtr = __SPEEDUP;
  __GmRight.cState = __MOTORSTOP;
  __GmLeft.cState = __MOTORSTOP;
  __GmRight.sSpeed = 0;
  __rightMotorContr();
  __GmLeft.sSpeed = 0;
  __leftMotorContr();
  __GmRight.uiPulseCtr = 10000;
  __GmLeft.uiPulseCtr = 10000;
  //mouseStop();
  //while(1);
}

void mouseTurnleft1(void) //第一次冲刺左转用
{

  GW = 0;
  time = 0;
  __GmWPID.sRef = 55;  //转弯角速度
  __GmSPID.sRef = 110; //转弯直线速度
  __GucMouseState = __TURNLEFT;
  __GmRight.cState = __MOTORRUN; //得加电脑鼠状态
  __GmLeft.cState = __MOTORRUN;
  GucMouseDir = (GucMouseDir + 3) % 4; /*  方向标记                    */
  while (1)
  {
    if (time > 290) //转弯角度调节259
      break;
  }
  __GmWPID.sRef = 0;
  __GucMouseState = __GOAHEAD;

  // mouseStop();
  //while(1);
  GuiSpeedCtr = 3;
  __GmLeft.uiPulse = 4000; //转弯之后，调节连续转弯用   11500
  __GmLeft.uiPulseCtr = 0;
  __GmRight.uiPulse = 4000; //转弯之后，调节连续转弯用   11500
  __GmRight.uiPulseCtr = 0;
  while ((__GmRight.uiPulseCtr + 200) <= __GmRight.uiPulse)
    ;
  while ((__GmLeft.uiPulseCtr + 200) <= __GmLeft.uiPulse)
    ;
  __GucMouseState = __TURNLEFT;
  GuiSpeedCtr = __SPEEDUP;
  __GmRight.cState = __MOTORSTOP;
  __GmLeft.cState = __MOTORSTOP;
  __GmRight.sSpeed = 0;
  __rightMotorContr();
  __GmLeft.sSpeed = 0;
  __leftMotorContr();
  __GmRight.uiPulseCtr = 10000;
  __GmLeft.uiPulseCtr = 10000;
  //mouseStop();
  //while(1);
}

void mouseTurnright2(void) //电脑鼠第二次冲刺右转用
{
  GW = 0;
  time = 0;
  __GmWPID.sRef = -60; //18
  __GmSPID.sRef = 115; //18

  __GucMouseState = __TURNRIGHT;

  __GmRight.cState = __MOTORRUN; //得加电脑鼠状态
  __GmLeft.cState = __MOTORRUN;
  GucMouseDir = (GucMouseDir + 1) % 4; /*  方向标记                    */
                                       /* __GucMouseState   = __GOAHEAD;//
   mouseStop();
   while(1); */
  while (1)
  {
    if (time > 283)
      break;
  }

  __GmWPID.sRef = 0;
  __GucMouseState = __GOAHEAD;
  //mouseStop();
  //while(1);

  //mouseStop();
  //while(1);
  GuiSpeedCtr = 3;
  __GmLeft.uiPulse = 3050; //9000
  __GmLeft.uiPulseCtr = 0;
  __GmRight.uiPulse = 3050;
  __GmRight.uiPulseCtr = 0;
  while ((__GmRight.uiPulseCtr + 200) <= __GmRight.uiPulse)
    ;
  while ((__GmLeft.uiPulseCtr + 200) <= __GmLeft.uiPulse)
    ;
  __GucMouseState = __TURNRIGHT;
  GuiSpeedCtr = __SPEEDUP;
  __GmRight.cState = __MOTORSTOP;
  __GmLeft.cState = __MOTORSTOP;
  __GmRight.sSpeed = 0;
  __rightMotorContr();
  __GmLeft.sSpeed = 0;
  __leftMotorContr();
  __GmRight.uiPulseCtr = 10000;
  __GmLeft.uiPulseCtr = 10000;
  // mouseStop();
  //while(1);
}

void mouseTurnleft2(void) //电脑鼠第二次冲刺左转用
{

  GW = 0;
  time = 0;
  __GmWPID.sRef = 60;  //18
  __GmSPID.sRef = 115; //18
  __GucMouseState = __TURNLEFT;
  __GmRight.cState = __MOTORRUN; //得加电脑鼠状态
  __GmLeft.cState = __MOTORRUN;
  GucMouseDir = (GucMouseDir + 3) % 4; /*  方向标记                    */
  while (1)
  {
    if (time > 286)
      break;
  }
  __GmWPID.sRef = 0;
  __GucMouseState = __GOAHEAD;

  // mouseStop();
  //while(1);
  GuiSpeedCtr = 3;
  __GmLeft.uiPulse = 2610; //14000
  __GmLeft.uiPulseCtr = 0;
  __GmRight.uiPulse = 2610;
  __GmRight.uiPulseCtr = 0;
  while ((__GmRight.uiPulseCtr + 200) <= __GmRight.uiPulse)
    ;
  while ((__GmLeft.uiPulseCtr + 200) <= __GmLeft.uiPulse)
    ;
  __GucMouseState = __TURNLEFT;
  GuiSpeedCtr = __SPEEDUP;
  __GmRight.cState = __MOTORSTOP;
  __GmLeft.cState = __MOTORSTOP;
  __GmRight.sSpeed = 0;
  __rightMotorContr();
  __GmLeft.sSpeed = 0;
  __leftMotorContr();
  __GmRight.uiPulseCtr = 10000;
  __GmLeft.uiPulseCtr = 10000;
  //mouseStop();
  // while(1);
}

void mouseStop(void)
{
  __GmSPID.sRef = 0; // 直线速度
  __GmWPID.sRef = 0; // 转弯速度
  GuiSpeedCtr = 5;   // 已达到速度？
}

void onestep(void)
{
  __GmSPID.sRef = 100;
  __GmRight.uiPulse = 20000;
  __GmRight.uiPulseCtr = 0;
  __GmLeft.uiPulse = 20000;
  __GmLeft.uiPulseCtr = 0;
  while ((__GmRight.uiPulseCtr + 200) <= __GmRight.uiPulse)
    ;
  while ((__GmLeft.uiPulseCtr + 200) <= __GmLeft.uiPulse)
    ;
}

void onestep1(void)
{
  __GmSPID.sRef = 100;
  __GmRight.uiPulse = 18000;
  __GmRight.uiPulseCtr = 0;
  __GmLeft.uiPulse = 18000;
  __GmLeft.uiPulseCtr = 0;
  while ((__GmRight.uiPulseCtr + 200) <= __GmRight.uiPulse)
    ;
  while ((__GmLeft.uiPulseCtr + 200) <= __GmLeft.uiPulse)
    ;
}

void mouseTurnback(void) //搜索掉头用，有后退
{

  if (GucFrontNear)
  {
    __GmSPID.sRef = 40;
    while ((ucIRCheck[0] < GusDistance_FL_Near) || (ucIRCheck[1] < GusDistance_FR_Near))
      ;
    __GucMouseState = __GOAHEAD;
    __GmRight.uiPulse = 1000;
    __GmRight.uiPulseCtr = 0;
    __GmLeft.uiPulse = 1000;
    __GmLeft.uiPulseCtr = 0;
    while ((__GmRight.uiPulseCtr + 200) <= __GmRight.uiPulse)
      ;
    while ((__GmLeft.uiPulseCtr + 200) <= __GmLeft.uiPulse)
      ;
  }
  backflag = 0;
  if ((ucIRCheck[2] > GusDistance_L_Near)) //偏左
  {
    backflag = 1;
  }
  GucFrontNear = 0;

  __GmSPID.sRef = 0;
  mouseStop();
  GW = 0;
  time = 0;

  __GucMouseState = __TURNBACK;
  __GmLeft.cState = __MOTORRUN;
  __GmRight.cState = __MOTORRUN;
  GucMouseDir = (GucMouseDir + 2) % 4;

  while (1)
  {
    if (time > 870) //掉头角度调节
      break;
  }
  __GmWPID.sRef = 0;
  mouseStop();
  __GucMouseState = __GOBACK;
  __GmSPID.sRef = -20;
  __GmRight.uiPulse = 15000; //150
  __GmRight.uiPulseCtr = 0;
  __GmLeft.uiPulse = 15000;
  __GmLeft.uiPulseCtr = 0;
  while ((__GmRight.uiPulseCtr + 200) <= __GmRight.uiPulse)
    ;
  while ((__GmLeft.uiPulseCtr + 200) <= __GmLeft.uiPulse)
    ;

  mouseStop();
  __GucMouseState = __GOAHEAD;
  __delay(1500000);

  GuiSpeedCtr = __SPEEDUP;
  __GmSPID.sRef = 80;
  __GucMouseState = __GOAHEAD;
  // mouseStop();
  //while(1);
  __GmRight.uiPulse = 35000; //掉头之后走的距离调节
  __GmRight.uiPulseCtr = 0;
  __GmLeft.uiPulse = 35000; //掉头之后走的距离调节
  __GmLeft.uiPulseCtr = 0;
  while ((__GmRight.uiPulseCtr + 200) <= __GmRight.uiPulse)
    ;
  while ((__GmLeft.uiPulseCtr + 200) <= __GmLeft.uiPulse)
    ;
  __GmRight.cState = __MOTORSTOP;
  __GmLeft.cState = __MOTORSTOP;

  __GmRight.sSpeed = 0;
  __rightMotorContr();
  __GmLeft.sSpeed = 0;
  __leftMotorContr();
  __GmRight.uiPulseCtr = 32000; //32000
  __GmLeft.uiPulseCtr = 32000;  //32000
                                // mouseStop();
                                //   while(1);
}

/*********************************************************************************************************
** Function name:       mouseTurnback_Y
** Descriptions:        原地旋转180度
** input parameters:    无
** output parameters:   无
** Returned value:      无
********************************** **********************************************************************/
void mouseTurnback_Y(void) //原地掉头不动程序
{

  __GmSPID.sRef = 0;
  mouseStop();
  __delay(1500000);

  GW = 0;
  time = 0;
  __GucMouseState = __TURNBACK;

  __GmLeft.cState = __MOTORRUN;
  __GmRight.cState = __MOTORRUN;
  GucMouseDir = (GucMouseDir + 2) % 4;
  while (1)
  {
    if (time > 850) //掉头角度调节
      break;
  }
  __GmWPID.sRef = 0;
  __GucMouseState = __GOAHEAD;
  mouseStop();
  __delay(1500000);

  __GmWPID.sRef = 0;

  __GmRight.cState = __MOTORSTOP;
  __GmLeft.cState = __MOTORSTOP;
  __GmRight.sSpeed = 0;
  __rightMotorContr();
  __GmLeft.sSpeed = 0;
  __leftMotorContr();
  __GmRight.uiPulseCtr = 0;
  __GmLeft.uiPulseCtr = 0;

  //  mouseStop();
  //    while(1);
}
void mouseTurnback_qidian(void) //终点掉头用，有后退
{

  __GmSPID.sRef = 0; // 直线速度
  mouseStop();
  __delay(1500000);

  GW = 0; // 小车转动角度
  time = 0;
  __GucMouseState = __TURNBACK;
  __GmLeft.cState = __MOTORRUN;
  __GmRight.cState = __MOTORRUN;
  GucMouseDir = (GucMouseDir + 2) % 4; // 当前方向
  while (1)
  {
    if (time > 750) //掉头角度调节750
      break;
  }
  __GmWPID.sRef = 0;
  __GucMouseState = __GOBACK;
  __GmSPID.sRef = -50;       //-50
  __GmRight.uiPulse = 16500; //后退距离，为了矫正 16500
  __GmRight.uiPulseCtr = 0;
  __GmLeft.uiPulse = 16500; //后退距离，为了矫正   16500
  __GmLeft.uiPulseCtr = 0;
  while ((__GmRight.uiPulseCtr + 200) <= __GmRight.uiPulse)
    ;
  while ((__GmLeft.uiPulseCtr + 200) <= __GmLeft.uiPulse)
    ;
  __GucMouseState = __GOAHEAD;
  mouseStop();
  __delay(6000000); //等待时间
  __GmWPID.sRef = 0;
  GuiSpeedCtr = __SPEEDUP;
  __GmSPID.sRef = 50;
  __GmRight.uiPulse = 16500; //前进距离，为了矫正
  __GmRight.uiPulseCtr = 0;
  __GmLeft.uiPulse = 16500; //前进距离，为了矫正
  __GmLeft.uiPulseCtr = 0;
  while ((__GmRight.uiPulseCtr + 200) <= __GmRight.uiPulse)
    ;
  while ((__GmLeft.uiPulseCtr + 200) <= __GmLeft.uiPulse)
    ;

  __GmRight.cState = __MOTORSTOP;
  __GmLeft.cState = __MOTORSTOP;
  __GmRight.sSpeed = 0;
  __rightMotorContr();
  __GmLeft.sSpeed = 0;
  __leftMotorContr();
  __GmRight.uiPulseCtr = 0;
  __GmLeft.uiPulseCtr = 0;
}

/*********************************************************************************************************
** Function name:       mazeSearch
** Descriptions:        前进N格
** input parameters:    iNblock: 前进的格数
** output parameters:   无
** Returned value:      无
*********************************************************************************************************/
void mazeSearch(void) //电脑鼠搜索程序
{
  int8 cL = 0, cR = 0, cCoor = 1;
  if (__GmLeft.cState)
  {
    cCoor = 0;
  }
  if ((__GucMouseState == __TURNRIGHT) || (__GucMouseState == __TURNLEFT))
  {
    __GmLeft.uiPulseCtr = 30000;
    __GmRight.uiPulseCtr = 30000;
    cL = 1;
    cR = 1;

    if (((__GucDistance[__FRONTR] != 0) &&
         (__GucDistance[__FRONTL] != 0)) ||
        ((__GucDistance[__LEFT] & 0x01) == 0) ||
        ((__GucDistance[__RIGHT] & 0x01) == 0))
    {
      if ((__GucDistance[__FRONTR] != 0) &&
          (__GucDistance[__FRONTL] != 0))
        GuiTpusle_LR = 14000;
      else
        GuiTpusle_LR = 12000;
    }
    else
    {
      GuiTpusle_LR = 0;
    }
  }
  else
  {
    GuiTpusle_LR = 0;
  }
  __GucMouseState = __GOAHEAD;
  __GiMaxSpeed = SEARCHSPEED;
  __GmRight.uiPulse = MAZETYPE * ONEBLOCK;
  __GmLeft.uiPulse = MAZETYPE * ONEBLOCK;
  __GmRight.cState = __MOTORRUN;
  __GmLeft.cState = __MOTORRUN;
  GuiSpeedCtr = __SPEEDUP;
  while (__GmLeft.cState != __MOTORSTOP)
  {

    if (__GmLeft.uiPulseCtr >= ONEBLOCK)
    { /*  判断是否走完一格*/
      __GmLeft.uiPulse -= ONEBLOCK;
      __GmLeft.uiPulseCtr -= ONEBLOCK;
      if (cCoor)
      {
        if (((__GucDistance[__FRONTR] != 0) && (__GucDistance[__FRONTL] != 0)) && (ucIRCheck[2] > GusDistance_L_Far) && (ucIRCheck[3] > GusDistance_R_Far)) //0x01
        {
          GucFrontNear = 1;

          goto End;
        }
        __mouseCoorUpdate(); /*  更新坐标                    */
      }
      else
      {
        cCoor = 1;
      }
    }
    if (__GmRight.uiPulseCtr >= ONEBLOCK)
    { /*  判断是否走完一格            */
      __GmRight.uiPulse -= ONEBLOCK;
      __GmRight.uiPulseCtr -= ONEBLOCK;
    }

    if (cL)
    { //搜索左转之间调试                                                    /*  是否允许检测左边            */
      if ((__GucDistance[__LEFT] & 0x01) == 0)
      { /*  左边有支路，跳出程序        */

        __GmRight.uiPulse = __GmRight.uiPulseCtr + 14100 - GuiTpusle_LR; //左转之前调节转弯时距离是否居中
        __GmLeft.uiPulse = __GmLeft.uiPulseCtr + 14100 - GuiTpusle_LR;   //左转之前调节转弯时距离是否居中15500
        while ((__GucDistance[__LEFT] & 0x01) == 0)
        {

          if ((__GmLeft.uiPulseCtr + 100) > __GmLeft.uiPulse)
          {

            goto End;
          }
        }
        __GmRight.uiPulse = MAZETYPE * ONEBLOCK;
        __GmLeft.uiPulse = MAZETYPE * ONEBLOCK;
        GuiSpeedCtr = __SPEEDUP;
      }
    }
    else
    { /*  左边有墙时开始允许检测左边  */
      if (ucIRCheck[2] > GusDistance_L_Far)
      {
        cL = 1;
      }
    }
    if (cR)
    { //搜索右转之间调试                                                       /*  是否允许检测右边            */
      if ((__GucDistance[__RIGHT] & 0x01) == 0)
      { /*  右边有支路，跳出程序        */

        __GmRight.uiPulse = __GmRight.uiPulseCtr + 13800 - GuiTpusle_LR; //右转之前调节转弯时距离是否居中14100
        __GmLeft.uiPulse = __GmLeft.uiPulseCtr + 13800 - GuiTpusle_LR;   //右转之前调节转弯时距离是否居中
        while ((__GucDistance[__RIGHT] & 0x01) == 0)
        {

          if ((__GmLeft.uiPulseCtr + 100) > __GmLeft.uiPulse)
          {
            goto End;
          }
        }
        __GmRight.uiPulse = MAZETYPE * ONEBLOCK;
        __GmLeft.uiPulse = MAZETYPE * ONEBLOCK;
        GuiSpeedCtr = __SPEEDUP;
      }
    }
    else
    {
      if (ucIRCheck[3] > GusDistance_R_Far)
      {
        cR = 1;
      }
    }
  }
End:

  __mouseCoorUpdate(); /*  更新坐标                    */
}
/****************************************************************************************************
** Function name:       mouseTurnright
** Descriptions:        右转
** input parameters:    无
** output parameters:   无
** Returned value:      无         按步数转弯
*********************************************************************************************************/
void mouseTurnright(void) //电脑鼠搜索右转程序
{

  GW = 0;
  time = 0;
  __GmWPID.sRef = -60; //转弯角速度
  __GmSPID.sRef = 110; //转弯直线速度
  __GucMouseState = __TURNRIGHT;
  __GmRight.cState = __MOTORRUN; //得加电脑鼠状态
  __GmLeft.cState = __MOTORRUN;
  GucMouseDir = (GucMouseDir + 1) % 4; /*  方向标记                    */
  while (1)
  {
    if (time > 270) //转弯角度调节，需把之后距离加大，去掉挡板来看280
      break;
  }

  __GmWPID.sRef = 0;
  __GucMouseState = __GOAHEAD;
  GuiSpeedCtr = 3;
  __GmLeft.uiPulse = 6200; //转弯之后的距离7000
  __GmLeft.uiPulseCtr = 0;
  __GmRight.uiPulse = 6200; //转弯之后的距离  7000
  __GmRight.uiPulseCtr = 0;
  while ((__GmRight.uiPulseCtr + 200) <= __GmRight.uiPulse)
    ;
  while ((__GmLeft.uiPulseCtr + 200) <= __GmLeft.uiPulse)
    ;
  __GucMouseState = __TURNRIGHT;
  GuiSpeedCtr = __SPEEDUP;
  __GmRight.cState = __MOTORSTOP;
  __GmLeft.cState = __MOTORSTOP;
  __GmRight.sSpeed = 0;
  __rightMotorContr();
  __GmLeft.sSpeed = 0;
  __leftMotorContr();
  __GmRight.uiPulseCtr = 10000;
  __GmLeft.uiPulseCtr = 10000;
  //mouseStop();
  //while(1);
}
/*********************************************************************************************************
** Function name:       mouseTurnleft
** Descriptions:        左转
** input parameters:    无
** output parameters:   无
** Returned value:      无
*********************************************************************************************************/
void mouseTurnleft(void) //电脑鼠搜索左转程序
{

  GW = 0;
  time = 0;
  __GmWPID.sRef = 60;  //转弯角速度
  __GmSPID.sRef = 110; //转弯直线速度
  __GucMouseState = __TURNLEFT;
  __GmRight.cState = __MOTORRUN; //得加电脑鼠状态
  __GmLeft.cState = __MOTORRUN;
  GucMouseDir = (GucMouseDir + 3) % 4; /*  方向标记                    */
  while (1)
  {
    if (time > 285) //转弯角度调节，需把之后距离加大，去掉挡板来看280
      break;
  }
  __GmWPID.sRef = 0;
  __GucMouseState = __GOAHEAD;
  GuiSpeedCtr = 3;
  __GmLeft.uiPulse = 6000; //转弯之后的距离  7000
  __GmLeft.uiPulseCtr = 0;
  __GmRight.uiPulse = 6000; //转弯之后的距离
  __GmRight.uiPulseCtr = 0;
  while ((__GmRight.uiPulseCtr + 200) <= __GmRight.uiPulse)
    ;
  while ((__GmLeft.uiPulseCtr + 200) <= __GmLeft.uiPulse)
    ;
  __GucMouseState = __TURNLEFT;
  GuiSpeedCtr = __SPEEDUP;
  __GmRight.cState = __MOTORSTOP;
  __GmLeft.cState = __MOTORSTOP;
  __GmRight.sSpeed = 0;
  __rightMotorContr();
  __GmLeft.sSpeed = 0;
  __leftMotorContr();
  __GmRight.uiPulseCtr = 10000;
  __GmLeft.uiPulseCtr = 10000;
  //mouseStop();
  // while(1);
}

/*********************************************************************************************************
** Function name:       __mouseCoorUpdate
** Descriptions:        根据当前方向更新坐标值
** input parameters:    无
** output parameters:   无
** Returned value:      无
*********************************************************************************************************/
void __mouseCoorUpdate(void)
{

  switch (GucMouseDir)
  {

  case 0:
    GmcMouse.cY++;
    break;

  case 1:
    GmcMouse.cX++;
    break;

  case 2:
    GmcMouse.cY--;
    break;

  case 3:
    GmcMouse.cX--;
    break;

  default:
    break;
  }

  __mazeInfDebug();
  __wallCheck();
}

void __mazeInfDebug(void)
{
  /*
     *  显示方向
     */
  switch (GucMouseDir)
  {

  case 0:
    zlg7289Download(2, 3, 0, 0x47); /*  向前，用F表示               */
    break;

  case 1:
    zlg7289Download(2, 3, 0, 0x77); /*  向右，用R表示               */
    break;

  case 2:
    zlg7289Download(2, 3, 0, 0x1f); /*  向后，用b表示               */
    break;

  case 3:
    zlg7289Download(2, 3, 0, 0x0e); /*  向左，用L表示               */
    break;

  default:
    zlg7289Download(2, 3, 0, 0x4f); /*  错误，用E表示               */
    break;
  }
  /*
     *  显示坐标
     */
  zlg7289Download(1, 0, 0, GmcMouse.cX / 10);
  zlg7289Download(1, 1, 0, GmcMouse.cX % 10);
  zlg7289Download(1, 6, 0, GmcMouse.cY / 10);
  zlg7289Download(1, 7, 0, GmcMouse.cY % 10);
}

/*********************************************************************************************************
** Function name:       __wallCheck
** Descriptions:        根据传感器检测结果判断是否存在墙壁
** input parameters:    无
** output parameters:   无
** Returned value:      cValue: 低三位从左到右一次代表左前右。1为有墙，0为没墙。
*********************************************************************************************************/
void __wallCheck(void)
{
  uint8 ucMap = 0;
  uint8 uctemp = 0;
  ucMap |= MOUSEWAY_B;

  if (ucIRCheck[2] > GusDistance_L_Far) //左边有挡板，但是距离较远
  {
    ucMap &= ~MOUSEWAY_L; //相对方向的左边有墙，并却转换成绝对坐标
    uctemp |= 0x06;
  }
  else
  {
    ucMap |= MOUSEWAY_L; //相对方向的左边无墙，并却转换成绝对坐标
  }

  if ((ucIRCheck[0] > GusDistance_FL_Far) && (ucIRCheck[1] > GusDistance_FR_Far))
  {
    ucMap &= ~MOUSEWAY_F;
    uctemp |= 0x40;
  }
  else
  {
    ucMap |= MOUSEWAY_F;
  }
  if (ucIRCheck[3] > GusDistance_R_Far)
  {
    ucMap &= ~MOUSEWAY_R;
    uctemp |= 0x30;
  }
  else
  {
    ucMap |= MOUSEWAY_R;
  }
  GucMapBlock0[GmcMouse.cX][GmcMouse.cY] = ucMap;
  GucMapBlock[GmcMouse.cX][GmcMouse.cY] = ucMap;
  GucMapBlock1[GmcMouse.cX][GmcMouse.cY] = ucMap;
  if (GmcMouse.cY < (MAZETYPE - 1))
  {
    GucMapBlock1[GmcMouse.cX][GmcMouse.cY + 1] &= ~(((~ucMap) & 0x01) * 4);
  } /*将该坐标周围坐标墙壁资料更改  注：洪水用*/
  if (GmcMouse.cX < (MAZETYPE - 1))
  {
    GucMapBlock1[GmcMouse.cX + 1][GmcMouse.cY] &= ~(((~ucMap) & 0x02) * 4);
  }
  if (GmcMouse.cY > 0)
  {
    GucMapBlock1[GmcMouse.cX][GmcMouse.cY - 1] &= ~(((~ucMap) & 0x04) / 4);
  }
  if (GmcMouse.cX > 0)
  {
    GucMapBlock1[GmcMouse.cX - 1][GmcMouse.cY] &= ~(((~ucMap) & 0x08) / 4);
  }

  if (GmcMouse.cY < (MAZETYPE - 1))
  {
    GucMapBlock[GmcMouse.cX][GmcMouse.cY + 1] |= ((ucMap & 0x01) * 4);
  } /*将该坐标周围坐标墙壁资料更改  注：在初始为有墙时管用*/
  if (GmcMouse.cX < (MAZETYPE - 1))
  {
    GucMapBlock[GmcMouse.cX + 1][GmcMouse.cY] |= ((ucMap & 0x02) * 4);
  }
  if (GmcMouse.cY > 0)
  {
    GucMapBlock[GmcMouse.cX][GmcMouse.cY - 1] |= ((ucMap & 0x04) / 4);
  }
  if (GmcMouse.cX > 0)
  {
    GucMapBlock[GmcMouse.cX - 1][GmcMouse.cY] |= ((ucMap & 0x08) / 4);
  }

  zlg7289Download(2, 2, 0, uctemp);
}

uint8 DenggaoCheck(void) //搜索中制作等高图stop，原地转弯判断
{
  if ((__GucDistance[__FRONT]) && (__GucDistance[__LEFT] & 0x01) && (__GucDistance[__RIGHT] & 0x01))
  {
    return (true);
  }
  else
  {
    return (false);
  }
}
uint8 PulseCtrCheck(void) ////搜索中制作等高图stop，已走步数判断
{
  if (__GmLeft.uiPulseCtr > (__GmLeft.uiPulse + 4000))
  {
    return (true);
  }
  else
  {
    return (false);
  }
}

void EXTI3_IRQHandler(void)
{
  char key;
  if (EXTI_GetITStatus(EXTI_Line3) != RESET)
  {
    key = zlg7289Key();
    if (key != 0xff)
    {
      if (key >= 4)
      {
        if (key == 10)
        {
          DIS[0] = GusDistance_L_Near;
          DIS[1] = GusDistance_R_Near;
          DIS[2] = GusDistance_L_Mid;
          DIS[3] = GusDistance_R_Mid;
          DIS[4] = GusDistance_L_Far;
          DIS[5] = GusDistance_R_Far;
          DIS[6] = GusDistance_FL_Far;
          DIS[7] = GusDistance_FR_Far;
          DIS[8] = GusDistance_FL_Near;
          DIS[9] = GusDistance_FR_Near;
          STMFLASH_Write(IR_SAVE_ADDRESS, (u16 *)DIS, 10);
          __delay(1000000);
        }

        else
        {
          Tab = key;
          ir_flag = 1;
        }
      }
      switch (Tab)
      {
      case 4:
        if (ir_flag == 1)
        {
          GusDistance_L_Near = ucIRCheck[2];
          GusDistance_R_Near = ucIRCheck[3];
          ir_flag = 0;
        }
        if (key == 1)
        {
          GusDistance_L_Near += 10;
          zlg7289Download(0, 4, 0, GusDistance_L_Near / 1000);
          zlg7289Download(0, 5, 0, GusDistance_L_Near / 100 % 10);
          zlg7289Download(0, 6, 0, GusDistance_L_Near / 10 % 10);
          zlg7289Download(0, 7, 0, GusDistance_L_Near % 10);
        }
        else if (key == 3)
        {
          GusDistance_L_Near -= 1;
          zlg7289Download(0, 5, 0, GusDistance_L_Near / 100);
          zlg7289Download(0, 6, 0, GusDistance_L_Near / 10 % 10);
          zlg7289Download(0, 7, 0, GusDistance_L_Near % 10);
        }
        else if (key == 0)
        {
          GusDistance_R_Near += 10;
          zlg7289Download(0, 5, 0, GusDistance_R_Near / 100);
          zlg7289Download(0, 6, 0, GusDistance_R_Near / 10 % 10);
          zlg7289Download(0, 7, 0, GusDistance_R_Near % 10);
        }
        else if (key == 2)
        {
          GusDistance_R_Near -= 1;
          zlg7289Download(0, 5, 0, GusDistance_R_Near / 100);
          zlg7289Download(0, 6, 0, GusDistance_R_Near / 10 % 10);
          zlg7289Download(0, 7, 0, GusDistance_R_Near % 10);
        }
        break;
      case 5:
        if (ir_flag == 1)
        {
          GusDistance_L_Mid = ucIRCheck[2];
          GusDistance_R_Mid = ucIRCheck[3];
          ir_flag = 0;
        }
        if (key == 1)
        {
          GusDistance_L_Mid += 10;
          zlg7289Download(0, 5, 0, GusDistance_L_Mid / 100);
          zlg7289Download(0, 6, 0, GusDistance_L_Mid / 10 % 10);
          zlg7289Download(0, 7, 0, GusDistance_L_Mid % 10);
        }
        else if (key == 3)
        {
          GusDistance_L_Mid -= 1;
          zlg7289Download(0, 5, 0, GusDistance_L_Mid / 100);
          zlg7289Download(0, 6, 0, GusDistance_L_Mid / 10 % 10);
          zlg7289Download(0, 7, 0, GusDistance_L_Mid % 10);
        }
        else if (key == 0)
        {
          GusDistance_R_Mid += 10;
          zlg7289Download(0, 5, 0, GusDistance_R_Mid / 100);
          zlg7289Download(0, 6, 0, GusDistance_R_Mid / 10 % 10);
          zlg7289Download(0, 7, 0, GusDistance_R_Mid % 10);
        }
        else if (key == 2)
        {
          GusDistance_R_Mid -= 1;
          zlg7289Download(0, 5, 0, GusDistance_R_Mid / 100);
          zlg7289Download(0, 6, 0, GusDistance_R_Mid / 10 % 10);
          zlg7289Download(0, 7, 0, GusDistance_R_Mid % 10);
        }
        break;
      case 6:
        if (ir_flag == 1)
        {
          GusDistance_L_Far = ucIRCheck[2];
          GusDistance_R_Far = ucIRCheck[3];
          ir_flag = 0;
        }
        if (key == 1)
        {
          GusDistance_L_Far += 10;
          zlg7289Download(0, 5, 0, GusDistance_L_Far / 100);
          zlg7289Download(0, 6, 0, GusDistance_L_Far / 10 % 10);
          zlg7289Download(0, 7, 0, GusDistance_L_Far % 10);
        }
        else if (key == 3)
        {
          GusDistance_L_Far -= 1;
          zlg7289Download(0, 5, 0, GusDistance_L_Far / 100);
          zlg7289Download(0, 6, 0, GusDistance_L_Far / 10 % 10);
          zlg7289Download(0, 7, 0, GusDistance_L_Far % 10);
        }
        else if (key == 0)
        {
          GusDistance_R_Far += 10;
          zlg7289Download(0, 5, 0, GusDistance_R_Far / 100);
          zlg7289Download(0, 6, 0, GusDistance_R_Far / 10 % 10);
          zlg7289Download(0, 7, 0, GusDistance_R_Far % 10);
        }
        else if (key == 2)
        {
          GusDistance_R_Far -= 1;
          zlg7289Download(0, 5, 0, GusDistance_R_Far / 100);
          zlg7289Download(0, 6, 0, GusDistance_R_Far / 10 % 10);
          zlg7289Download(0, 7, 0, GusDistance_R_Far % 10);
        }
        break;
      case 7:
        if (ir_flag == 1)
        {
          GusDistance_FL_Far = ucIRCheck[0];
          GusDistance_FR_Far = ucIRCheck[1];
          ir_flag = 0;
        }
        if (key == 1)
        {
          GusDistance_FL_Far += 10;
          zlg7289Download(0, 5, 0, GusDistance_FL_Far / 100);
          zlg7289Download(0, 6, 0, GusDistance_FL_Far / 10 % 10);
          zlg7289Download(0, 7, 0, GusDistance_FL_Far % 10);
        }
        else if (key == 3)
        {
          GusDistance_FL_Far -= 1;
          zlg7289Download(0, 5, 0, GusDistance_FL_Far / 100);
          zlg7289Download(0, 6, 0, GusDistance_FL_Far / 10 % 10);
          zlg7289Download(0, 7, 0, GusDistance_FL_Far % 10);
        }
        else if (key == 0)
        {
          GusDistance_FR_Far += 10;
          zlg7289Download(0, 5, 0, GusDistance_FR_Far / 100);
          zlg7289Download(0, 6, 0, GusDistance_FR_Far / 10 % 10);
          zlg7289Download(0, 7, 0, GusDistance_FR_Far % 10);
        }
        else if (key == 2)
        {
          GusDistance_FR_Far -= 1;
          zlg7289Download(0, 5, 0, GusDistance_FR_Far / 100);
          zlg7289Download(0, 6, 0, GusDistance_FR_Far / 10 % 10);
          zlg7289Download(0, 7, 0, GusDistance_FR_Far % 10);
        }
        break;
      case 8:
        if (ir_flag == 1)
        {
          GusDistance_FL_Near = ucIRCheck[0];
          GusDistance_FR_Near = ucIRCheck[1];
          ir_flag = 0;
        }
        if (key == 1)
        {
          GusDistance_FL_Near += 10;
          zlg7289Download(0, 5, 0, GusDistance_FL_Near / 100);
          zlg7289Download(0, 6, 0, GusDistance_FL_Near / 10 % 10);
          zlg7289Download(0, 7, 0, GusDistance_FL_Near % 10);
        }
        else if (key == 3)
        {
          GusDistance_FL_Near -= 1;
          zlg7289Download(0, 5, 0, GusDistance_FL_Near / 100);
          zlg7289Download(0, 6, 0, GusDistance_FL_Near / 10 % 10);
          zlg7289Download(0, 7, 0, GusDistance_FL_Near % 10);
        }
        else if (key == 0)
        {
          GusDistance_FR_Near += 10;
          zlg7289Download(0, 5, 0, GusDistance_FR_Near / 100);
          zlg7289Download(0, 6, 0, GusDistance_FR_Near / 10 % 10);
          zlg7289Download(0, 7, 0, GusDistance_FR_Near % 10);
        }
        else if (key == 2)
        {
          GusDistance_FR_Near -= 1;
          zlg7289Download(0, 5, 0, GusDistance_FR_Near / 100);
          zlg7289Download(0, 6, 0, GusDistance_FR_Near / 10 % 10);
          zlg7289Download(0, 7, 0, GusDistance_FR_Near % 10);
        }
        break;

      default:
        break;
      }
      __delay(200000);
      EXTI_ClearITPendingBit(EXTI_Line3);
    }
  }
}